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ABSTRACT 


Robot  motion  planning  with  differential  constraints  has  received  a  great  deal  of  attention  in  the  last 
few  decades,  yet  it  still  remains  a  challenging  problem.  Among  a  number  of  reasons,  three  stand 
out.  First,  the  differential  constraints  that  most  physical  robots  exhibit  render  the  coupling  between 
the  control  and  state  spaces  quite  complicated.  Second,  it  is  commonly  accepted  that  robots  must 
be  able  to  operate  in  environments  that  are  partially  or  entirely  unknown;  classical  motion  planning 
techniques  that  assume  known  structure  of  the  world  frequently  encounter  difficulties  when  applied 
in  this  setting.  Third,  such  robots  are  typically  expected  to  operate  with  speed  that  is  commensurate 
with  that  of  humans.  This  poses  stringent  limitations  on  available  runtime  and  often  hard  real-time 
requirements  on  the  motion  planner.  The  impressive  advances  in  computing  capacity  in  recent 
years  have  been  unable,  by  themselves,  to  meet  the  computational  challenge  of  this  problem.  New 
algorithmic  approaches  to  tackle  its  difficulties  continue  to  be  developed  to  this  day. 

The  approach  advocated  in  this  thesis  is  based  on  encapsulating  some  of  the  complexity  of 
satisfying  the  differential  constraints  in  pre-computed  controls  that  serve  as  motion  primitives, 
elementary  motions  that  are  combined  to  form  the  solution  trajectory  for  the  system.  The  contri¬ 
bution  of  this  work  is  in  developing  a  general  approach  to  constructing  such  motion  primitives, 
given  a  model  of  robot  mobility.  Moreover,  the  approach  allows  an  unprecedented  amount  of 
pre-computation  in  this  domain,  which  yields  a  dramatic  increase  in  planning  efficiency  even  for 
systems  with  complex  kinematics  and  dynamics.  Lastly,  the  proposed  motion  primitives  are  fully 
compatible  with  a  wide  range  of  planning  algorithms  and  allow  such  useful  techniques  as  incre- 
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mental  planning  and  multi-directional  search  to  be  used  in  the  context  of  planning  with  differential 
constraints. 

These  ideas  are  demonstrated  and  validated  on  a  number  of  relevant  systems,  both  in  simula¬ 
tion  and  in  real  experiments.  This  work  promises  to  enable  capable  and  reliable  motion  planners 
with  differential  constraints,  as  encountered  in  many  realistic  robot  systems  with  practical  utility, 
operating  efficiently  in  cluttered,  partially  known  environments. 
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CHAPTER 


INTRODUCTION 


Motion  planning  with  differential  constraints  is  recognized  as  a  challenging  problem  in  robotics. 
The  search  space  dimensionality,  system  dynamics,  uncertainty  and  partial  knowledge  about  the 
environment  further  contribute  to  the  difficulty  of  this  problem.  Solving  it  in  real  time  -  i.e.  on  a 
time  scale  commensurate  with  human  motion  -  is  even  more  challenging.  In  addition,  on-board 
computation  of  mobile  robots  is  less  powerful  than  that  of  static,  immobile  systems.  Yet  a  wide 
variety  of  real  robots,  from  wheeled  systems  to  air  and  water  vehicles,  rely  on  this  type  of  motion 
planning  to  move  efficiently  enough  to  be  useful  in  their  applications.  Despite  several  decades  of 
active  work  in  this  field,  it  is  still  an  open  area  of  research. 

The  approach  taken  in  this  thesis  is  to  reduce  the  computation  complexity  of  on-line,  on-board 
motion  planning  with  differential  constraints  by  performing  as  much  off-line  pre-computation  as 
possible.  System  reachability  is  analyzed  a  priori,  and  the  constraints  are  encapsulated  in  mo¬ 
tion  primitives,  pre-computed  trajectories  that  are  known  to  be  feasible  motions.  While  it  is  not 
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tractable  to  pre-compute  all  possible  trajectories  for  the  system,  this  thesis  attempts  to  generate 
representative  sets  of  primitives  that  capture  the  maneuverability  of  the  system  as  well  as  possible 
given  a  model  of  robot’s  mobility.  Motion  planning  is  then  formulated  as  searching  a  graph  that 
includes  these  motion  primitives  as  atomic  state  transitions.  In  this  manner,  motion  planning  with 
differential  constraints  is  reduced  to  fast  unconstrained  search  in  the  space  that  encapsulates  the 
constraints  and  is  generated  via  significant,  but  off-line,  pre-computation.  The  objective  of  this 
thesis  is  the  design  of  motion  primitives  that  lead  to  efficient  planning  in  this  setting. 

While  motion  primitives  have  been  advocated  in  motion  planning  in  the  past  [44;  99],  this 
thesis  provides  an  innovative  solution  in  several  respects.  First,  the  structure  of  the  primitives  pro¬ 
posed  herein  makes  them  compatible  with  an  unprecedented  variety  of  efficient  search  techniques, 
including  incremental  and  multi-directional  search  -  techniques  that  constrained  planning  of  this 
type  is  rarely  able  to  benefit  from.  This  is  important  for  minimizing  computational  requirements,  as 
well  as  for  enabling  the  planner  to  react  to  the  inevitable  and  frequent  changes  in  the  environment 
representation  that  are  commonplace  in  robotics. 

Second,  the  approach  leads  to  a  greater  generality  of  application  and  ease  of  development 
of  motion  primitives.  Unlike  many  preceding  approaches,  the  proposed  motion  primitives  are 
computed  automatically,  given  a  model  of  the  system  mobility.  In  fact,  they  can  even  be  modified 
by  the  robot  itself,  should  there  be  a  significant  change  in  the  system,  e.g.  due  to  a  mechanical 
failure. 

This  thesis  attempts  to  demonstrate  that  the  proposed  motion  primitives  lead  to  new  gains  in 
motion  planning  by  reducing  computational  requirements  and  enabling  real-time  motion  planning 
with  differential  constraints  with  the  on-board  computing  capacity  of  autonomous  robot  systems, 
operating  in  real  environments  and  facing  the  multitude  of  challenges  that  are  inevitable  in  this 
setting. 

1.1  Motivation 

Motion  planning  with  differential  constraints  encompasses  nonholonomic  and  kinodynamic  plan¬ 
ning  and  is  recognized  as  an  important  and  hard  problem  in  robotics  [22].  A  great  variety  of 
approaches  is  being  proposed  to  this  day  ([11;  12;  29;  45;  58;  76;  81;  87;  92;  106]  among  others). 
Differential  constraints  are  important  to  consider  in  motion  planning  because  many  relevant  types 
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Figure  1.1.1:  Several  examples  of  autonomous  robots.  Some  of  the  project  and  robot  names  for 
which  I  had  the  privilege  of  developing  motion  planners,  from  top  left  to  right:  PerceptOR,  FIDO 
Continuous  Navigation,  ARM-S,  LAGR,  UPI,  Single-Cycle  Instrument  Placement  on  RockyS. 

of  robotics  platforms  impose  them.  Systems  such  as  wheeled  car-like  vehicles  and  tractor-trailer 
combinations  are  classical  examples  of  such  kinematics  constraints  [97;  98;  127].  In  addition,  most 
physical  robots  moving  at  speed  are  subject  to  dynamics  effects  due  to  inertia,  e.g.  drift,  which  also 
results  in  velocity  constraints.  Humanoid  walking  robots  [95],  needle  steering  [4;  57]  and  other 
systems  have  been  noted  to  exhibit  constraints  of  this  type  as  well. 

Even  though  differential  constraints  are  commonplace  in  robotics,  their  degree  of  severity 
varies.  Many  applications  attempt  to  ignore  such  constraints  because  that  significantly  simpli¬ 
fies  motion  planning.  It  is  often  done  by  relaxing  the  requirements  on  the  robot  (reducing  speed, 
simplifying  robot’s  environment),  by  pushing  the  complexity  to  the  robot’s  hardware  (e.g.  om¬ 
nidirectional  wheels,  etc.)  or  by  attempting  to  build  motion  controllers  that  would  eliminate  the 
disturbances  caused  by  ignoring  such  constraints  at  planning  time  (although,  in  principle,  this  is  as 
difficult  as  the  original  problem). 

While  in  some  applications  these  strategies  are  reasonable  ones,  the  resulting  performance 
losses  are  too  great  in  a  variety  of  relevant  scenarios.  The  examples  of  systems  that  cannot  afford 
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to  ignore  their  mobility  constraints  include  mobile  robots  with  large  minimum  turning  radius,  such 
as  autonomous  construction  and  mining  equipment  (at  the  time  of  this  writing,  being  developed 
by  research  [34]  and  commercial  communities,  e.g.  Caterpillar®,  Volvo®,  etc.),  tractor-trailer 
systems  [97]  and  indoor  robots  pushing  carts  [124].  In  addition,  there  are  a  variety  of  systems 
exhibiting  significant  dynamics  effects,  such  as  vehicles  moving  at  high  speed  [26],  flying  robots 
[38],  autonomous  boats  [56],  etc.  Ignoring  the  differential  constraints  of  such  systems  will  lead 
to  substantially  lower  productivity  than  what  these  platforms  are  capable  of,  due  to  overly  conser¬ 
vative  operation.  In  the  worst  case,  a  complete  failure,  perhaps  resulting  in  collision  and  damage 
to  the  vehicle,  will  occur  as  the  robot  controller  becomes  unable  to  execute  a  trajectory  that  was 
planned  without  regard  to  the  system’s  differential  constraints. 

Besides  its  relevance  in  applications,  motion  planning  with  differential  constraints  is  a  very  in¬ 
teresting  and  difficult  theoretical  problem.  There  are  at  least  four  specific  considerations  that  make 
it  especially  challenging  in  the  context  of  robotics.  First,  in  its  general  formulation,  this  problem  is 
recognized  to  be  VF-hard  [22].  Exact  solutions  only  exist  for  point  masses  with  bounds  on  ve¬ 
locity  and  acceleration  in  one-dimension  [102]  and  two-dimensions  [23].  Second,  robots  typically 
must  be  able  to  operate  with  speeds  commensurate  with  those  of  human  operators.  This  poses 
stringent  limitations  on  available  runtime,  and  often  hard  real-time  computation  requirements,  a 
related  topic  in  computer  science  research.  Third,  the  relationship  between  the  control  and  state 
spaces  of  the  robot  under  differential  constraints  is  quite  complicated.  In  some  cases,  even  simulat¬ 
ing  the  system  at  the  desired  fidelity  level  becomes  prohibitively  expensive  [136].  Fourth,  robot’s 
environments  are  typically  partially  or  entirely  unknown.  Moreover,  models  of  these  environments 
change  frequently  and  continuously  due  to  modeling  artifacts  that  are  difficult  or  impossible  to 
eliminate,  such  as  sensor  noise  [64].  In  this  setting,  many  classical  motion  planning  techniques 
that  assume  known  and  static  structure  of  the  world  face  significant  challenges. 

1.2  Problem  Statement 

Now  that  the  problem  of  motion  planning  with  differential  constraints  has  been  motivated,  it  is 
defined  here  in  concrete  terms.  First,  relevant  nomenclature  is  introduced  in  the  following  section, 
followed  by  an  identification,  in  Section  1.2.2,  of  four  specific  aspects  of  the  problem  that  make  it 
a  challenging  one  and  that  will  guide  the  development  of  the  presented  solution.  Finally,  Section 


CHAPTER  1.  INTRODUCTION 


5 


1.2.3  establishes  the  scope  of  the  inquiry  by  identifying  other  related,  though  separate,  problems. 

1.2.1  Terminology 

This  section  describes  several  concepts  that  are  central  to  this  thesis  and  defines  a  number  of 
related  terms  and  symbols.  Specifically,  it  defines  the  notions  of  robot  state,  control,  trajectories, 
paths  and  their  relation  to  the  problem  of  motion  planning  with  differential  constraints,  which  is 
also  rigorously  formulated  here. 

State  Space 

The  robot  state  space,  X,  is  a  representation  of  a  robot’s  location  in  the  world,  its  configuration, 
velocity  and  other  aspects  of  its  condition,  for  example:  position  (x,y,z)  and  orientation  ((|),0,\|/), 
linear  and  angular  velocities  ivx,  Vy,  v^,  (O^,  (Oy,  (Oz),  configurations  of  joints  (yo . . .  jn)  such  as  steer¬ 
ing  angle  of  a  car-like  vehicle,  etc. 

Special  symbols  denoting  certain  elements  of  state  space  include  the  initial  state,  xi  G  X,  and 
final  state,  xp  &X  of  the  vehicle.  These  are  also  known  as  boundary  conditions,  as  the  goal  of 
motion  planning  is  to  find  a  motion  that  steers  the  system  from  xi  to  xp.  Some  intermediate  state 
along  the  trajectory  is  denoted  as  Xi  eX. 

In  many  applications,  a  vehicle  must  avoid  certain  states.  This  may  be  due  to  obstacles  in  the 
environment  that  would  make  a  collision  dangerous  for  the  robot,  e.g.  mechanically.  There  may 
also  be  states  of  inevitable  collision  that,  while  not  being  in  collision,  would  invariably  lead  the 
vehicle  to  collide,  e.g.  due  to  dynamics.  There  may  also  be  arbitrary,  user-defined,  no-go  regions, 
e.g.  areas  of  loose  soil  where  the  vehicle  would  be  likely  to  get  stuck.  We  denote  by  Yobs  C  Y  a  set 
of  all  such  forbidden  states.  We  also  define  Yfree  =  Y  \  Yobs,  a  set  of  states  that  are  free  from  such 
limitations  and  are  allowed  for  the  vehicle  to  be  in. 

Control  Space 

Robots  are  physical  devices  that  react  to  the  influence  of  excitation  signals,  such  as  mechanical, 
electrical  or  other  types.  A  set  of  such  signals,  or  inputs,  comprises  the  control  space,  U  C  M™,  of 
the  robot. 
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Figure  1.2.1:  Car-like  system  model.  The  figure  shows  a  simple  system  model  that  represents  the 
kinematics  of  a  typical  four-wheeled  vehicle  with  car-like  mobility.  The  variables  x  and  y  are  the 
2D  coordinates  of  a  reference  point  of  the  vehicle  with  respect  to  a  Cartesian  frame,  and  0  is  the 
angle  between  the  positive  .r-axis  and  the  main  axis  of  the  vehicle;  (|)  is  the  steering  angle. 


The  set  of  inputs  depends  on  the  platform  and  implementation.  A  wheeled  mobile  robot  can  be 
controlled  directly  in  the  input  space  of  wheel  torques  or  at  a  higher  level,  e.g.  body-frame  linear 
and  angular  velocities,  where  wheel  torques  are  computed  by  a  controller. 


System  Model 

A  robot  is  modeled  as  a  system,  a  set  of  interacting  components  that  reacts  in  certain  ways  to  the 
control  signals  applied  to  it.  In  this  thesis,  this  system  is  represented  by  a  time-invariant  differential 
equation: 


x  =  f{x,u)  (1.2.1) 

where  x  EX,  u  eU  ,  and  i  =  ^  is  the  time  derivative.  This  equation  is  referred  to  as  a  system 
model,  also  known  as  a  state  transition  equation.  It  encapsulates  the  differential  constraints  of  the 
robot.  One  basic  example  of  a  transition  equation  is  given  below. 
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Example  1.2.1  (Car-like  Transition  Equation).  A  basic  model  of  a  robot  with  car-like  mobility  is 
shown  in  Figure  1.2.1,  adapted  from  [128].  State  space  of  the  car  is  three-dimensional: 

[x,y,QY  ex  = 

where  x  and  y  are  the  2D  coordinates  of  a  reference  point  of  the  vehicle  with  respect  to  a  Cartesian 
frame,  and  0  is  the  angle  between  the  positive  x-axis  and  the  robot’s  main  axis.  The  control  space 
is  two-dimensional: 


[m  1 ,  M2]  ^  C  U  = 

where  ui  is  linear  velocity  of  the  car,  and  U2  is  its  steering  angle. 

The  model  features  two  types  of  differential  constraints.  One  expresses  rolling  of  the  wheels 
without  slipping  which  causes  the  vehicle  to  move  tangentially  to  its  main  axis.  This  non-integrable 
equality  constraint  is  expressed  by  the  following  equation 

ycosG  —  isinG  =  0 

In  addition,  there  is  a  non-integrable  inequality  constraint  that  represents  a  mechanical  limit 
on  the  steering  angle  of  the  car: 


\  >  |tan(M2)| 

where  L  is  the  wheelbase  and  R  is  the  minimum  turning  radius  of  the  car. 

With  these  definitions,  the  transition  equation  (1.2.1)  for  the  car-like  robot  takes  the  form: 


X 

cosG 

'o' 

y 

= 

sinG 

Ml  -p 

0 

_0_ 

0 

L 

-R. 

U2 


(1.2.2) 
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Control 

A  control  is  a  continuous  mapping  from  time  to  control  space,  m  :  M  — )■  t/;  in  other  words,  the 
trajectory  specifies  a  control  signal  to  apply  to  the  system  at  a  given  point  in  time.  The  domain 
of  this  function  is  a  closed  interval  [tijp]  G  M,  where  ti  is  starting  time,  and  tp  is  final  time  of  the 
trajectory.  Note  that  the  symbol  u,  when  used  as  a  vector,  still  denotes  a  value  of  input,  an  element 
of  U,  whereas  when  written  as  a  function,  u{t),  it  denotes  a  control. 

Time  is  the  most  typical  trajectory  parametrization  because  time  is  monotonically  increasing, 
continuous  and  always  defined.  Other  parameterizations,  such  as  distance  traveled,  can  also  be 
utilized.  Without  loss  of  generality,  we  can  also  re-parameterize  a  trajectory  such  that  its  domain 
is  an  interval  [0, 1]  G  M. 

For  convenience,  we  define  a  function  space  of  all  controls,  Zl,  that  can  be  input  into  the  system 
for  execution. 

Trajectory 

A  trajectory,  :  A  x  M  — )■  A  is  a  projection  of  a  control  on  state  space  under  the  state  transition 
equation  (1.2.1)  and  is  obtained  by  integration: 

'tuixi:t)=  [  f{x{t^),u{t^))dt^ +  Xj  (1.2.3) 

Jtl 

Equation  1.2.3  cannot  be  integrated  in  closed  form  for  most  non-trivial  robot  systems.  Nu¬ 
merical  methods  such  as  Euler  or  multi-step  Runge-Kutta  must  be  applied  instead.  Balancing  the 
fidelity  of  this  integration  with  runtime  performance  is  one  of  the  challenges  of  motion  planning.  If 
the  required  simulation  fidelity  is  high,  this  model  integration,  also  referred  to  as  simulation,  may 
slow  down  planning  significantly.  The  projection  of  the  trajectory  on  the  position  subspace  of  A, 
e.g.  coordinates  [x,yY  in  Example  1.2.1  ,  is  referred  to  as  a  path. 

A  cusp  is  a  point  in  a  path  where  linear  velocity  changes  sign.  While  cusps  are  discontinuous 
in  path  curvature,  they  are  not  discontinuous  in  control  space  and  are  feasible  motions.  Similarly, 
a  turn-in-place  is  a  path  point  where  the  heading  changes  discontinuously,  however  a  turn-in-place 
is  continuous  in  the  control  space  trajectory  representation. 

For  convenience,  we  define  a  function  space  of  all  feasible  trajectories  T  that  result  from 
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applying  equation  (1.2.3)  to  every  element  of  U. 

Boundary  Value  Problem 

Boundary  value  problem  (BVP)  is  a  problem  of  computing  a  control  u{t)  G  Z1  such  that  the  result¬ 
ing  trajectory  satisfies  boundary  equality  constraints  and  the  state  transition  equation: 


i  =  /(x,  u) 

(1.2.4) 

'^uixidi)  =  XI 

(1.2.5) 

'^uixptp)  =  xp 

(1.2.6) 

However,  notice  that  u{t)  may  not  be  unique.  We  can  also  define  optimal  BVP  as  the  problem 
of  computing  a  trajectory  that  minimizes  a  certain  time-independent  cost  function  c  :  X  x  U  — )■  M 
that  assigns  cost  of  giving  the  system  an  input  u  &  U  while  it  is  in  state  x  gX.  In  practice,  cost  may 
be  related  to  distance  traveled  by  the  vehicle,  energy  consumed,  risk  experienced,  etc.  The  cost 
function  is  typically  designed  such  that  the  robot’s  perceptual  information  is  mapped  into  a  scalar 
value  that  is  consistent  with  the  desired  notion  of  motion  cost. 

Optimal  BVP  is  then  formulated  as: 

rtp 

u*  =  argmin  /  c{Xu{xi,t),u{t))dt 
ueu  Jti 

s.t.  x  =  f{x,u)  (1.2.7) 

'^u{xpti)  =xi 

'^uixptp)  =Xf 

The  domain  of  minimization  above,  U,  is  known  as  a  search  space  -  a  space  of  motion  alter¬ 
natives  that  is  evaluated  while  the  optimal  motion  is  sought. 

For  several  special  classes  of  systems,  such  solutions  are  available  in  closed  form  [30;  115].  In 
general,  however,  numerical  techniques  must  be  utilized.  We  observe  that  system  simulation  is  an 
important  component  of  this  problem  because  it  is  required  to  obtain  x„(t)  as  part  of  evaluating  the 
cost  integral  in  (1.2.7). 
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Figure  1.2.2:  Controller  path  following.  A  receding  horizon  controller  attempts  to  steer  the 
system  to  remain  on  the  reference  path  Xu{x)  (dashed  line)  by  generating  an  appropriate  correction 
control  (solid  line)  to  eliminate  the  cross-track  error.  The  figure  shows  a  popular  approach  that  is 
similar  to  pure-pursuit:  a  reference  trajectory  sample  Xi  is  chosen  at  the  L2  horizon,  or  look-ahead, 
radius  Rh-  In  this  setting,  computing  the  correction  control  is  identical  to  BVP. 


Controller 

As  argued  in  Section  1.1,  most  real  problems  in  robotics  face  challenges  of  uncertainty  and  unmod¬ 
elled  perturbations  both  in  the  robot’s  environment  and  in  the  robot’s  transition  equation.  Ignoring 
these  effects  may  cause  undesirable  interaction  with  the  environment,  e.g.  collision  potentially 
resulting  in  damage,  or  an  error  in  integrating  the  transition  equation,  which  may  grow  very  large 
over  the  course  of  executing  a  trajectory. 

In  order  to  mitigate  such  effects,  it  is  common  practice  to  equip  robot’s  actuators  with  a  module 
that  monitors  the  evolution  of  the  system  with  respect  to  a  path  that  it  is  attempting  to  follow 
during  motion,  referred  to  as  a  reference  trajectory,  and  issues  corrective  action  in  the  event  that 
the  system  deviates  from  this  trajectory.  This  module,  a  controller,  can  be  expressed  as  a  process 
that  implements  a  trajectory-following  policy  by  generating  a  control  based  on  error  dynamics 
during  trajectory  following. 

Figure  1.2.2  illustrates  that  designing  a  receding  horizon  controller  is  similar  to  solving  the 
BVP:  if  the  controller  is  built  to  utilize  certain  look-ahead  along  the  reference  trajectory,  then  its 
operation  is  similar  to  solving  the  BVP  to  steer  the  system  to  a  point  on  the  reference  trajectory 
that  is  within  the  given  time  horizon.  Based  on  the  close  relationship  between  the  two  concepts, 
the  term  controller  will  be  generalized  to  include  solving  the  BVP  in  the  sequel. 
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Example  1.2.2  (Motivating  feasible  planning).  The  car-like  robot,  just  like  the  one  in  Example 
1.2.1  attempts  to  travel  to  a  goal  on  the  other  side  of  a  collection  of  obstacles  that  forms  a  narrow 
corridor  (Figure  1.2.3).  Suppose  the  motion  planner  is  based  on  searching  a  grid,  and  thus  is 
produces  a  trajectory  through  the  corridor  (black  line).  However,  this  path  is  infeasible,  since  the 
car-like  robot  cannot  turn  in  place.  As  the  vehicle  moves  and  the  controller  attempts  to  guide  the 
vehicle  through  the  corridor,  it  is  not  able  to  do  so.  The  only  viable  alternative  is  to  back  up  and 
replan,  thereby  bringing  the  corridor  out  of  the  scope  of  the  controller  and  back  into  the  scope  of 
the  (infeasible)  planner,  and  the  process  repeats.  Without  special  behavior  monitoring,  the  vehicle 
will  cycle  back  and  forth  indefinitely.  □ 


Figure  1.2.3:  Motivating  feasible  planning.  An  example  of  a  motion  planning  problem,  where 
following  the  infeasible  planned  trajeetory  (blaek  line)  eauses  a  failure.  A  eontroller  is  unable  to 
“fix”  the  trajectory  to  enable  the  car-like  robot  to  follow  the  path.  Making  this  correction  is  as 
difficult  as  the  original  motion  planning  problem. 


Motion  Planning 

Notice  that  the  BVP  considers  differential  constraints  of  the  system  (via  the  state  transition  equa¬ 
tion)  and  the  boundary  state  constraints,  but  it  does  not  consider  environment  constraints,  such  as 
collision  avoidance.  We  formulate  the  motion  planning  problem  as  that  of  satisfying  the  boundary 
constraints  while  minimizing  a  cost  function  and  assuring  that  the  motion  is  collision  free. 
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ftp 

u*  =  argmin  /  c{Xu{t),u{t))dt 
ueii  Jii 

s.t.  x  =  f{x,u) 

/  .  N  (1-2.8) 

Tu{xidl)=Xi 

'^uixidp)  =Xf 

Xuixid)  GXfree,V?  G  [tiM 

In  most  real  robotics  applications,  the  model  of  the  environment,  including  Xf^e,  is  partially  un¬ 
known  and  uncertain.  Therefore,  the  robot  may  not  have  all  the  information  necessary  to  compute 
an  optimal  x*.  The  robot  is  only  able  to  compute  information-optimal  solution,  based  on  the  in¬ 
formation  that  it  does  have.  This  circumstance  is  frequently  omitted  in  motion  planning  literature, 
although  it  is  an  extremely  important  one  in  robotics.  As  the  robot’s  knowledge  of  the  environ¬ 
ment  improves,  the  robot  may  need  to  replan,  i.e.  modify  its  motion  plan  per  new  information 
[129].  Methods  that  are  able  to  improve  the  efficiency  of  replanning,  e.g.  due  to  pre-computation 
or  caching  previous  on-line  computation,  are  especially  promising  in  this  domain  -  which  has  been 
one  of  the  guiding  principles  in  this  work. 

Roadmap 

While  the  motion  planning  problem  with  differential  constraints,  as  defined  above,  remains  an 
open  research  question,  a  variety  of  approximate  solutions  have  been  proposed  over  the  last  several 
decades.  The  approach  proposed  in  this  thesis  is  most  similar  to  a  class  of  methods  referred  to  as 
roadmaps  [5;  61].  Such  methods  perform  sampling  in  A  in  order  to  build  up  a  directed,  cyclic  graph 
g  =  ‘V’U'E,  where  T'  G  A  is  a  set  of  vertices  and  £  G  T  is  a  set  of  trajectories  between  vertices, 
edges.  The  cyclic  property  of  these  graphs  distinguishes  them  from  the  approaches  based  on  trees 
[55;  87].  We  will  also  use  the  term  lazy  roadmap,  referring  to  the  practice  of  constructing  the 
roadmap  without  regard  for  obstacles  and  deferring  collision  detection  until  the  graph  is  searched 
[19].  Quasi-random  lazy  roadmaps  arrange  vertices  ‘V  with  specific  structure,  e.g.  a  lattice,  such 
that  the  roadmap  need  not  be  constructed  explicitly  before  search  [21]. 

The  roadmap  can  be  interpreted  as  a  discrete  set  of  states  that  are  reachable  by  the  system.  The 
edges  of  the  graph  are  weighted  in  such  a  way  as  to  represent  the  costs  of  traversing  them  and  are 
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based  on  the  values  of  the  cost  function.  The  state  values  of  successor  vertices  of  any  vertex  are 
obtained  by  computing  (simulating)  the  path  originating  at  the  vertex  using  the  control  trajectory 
associated  with  each  corresponding  edge. 

A  set  of  successor  edges  from  a  vertex  is  referred  to  as  a  control  set.  In  general,  each  vertex 
has  its  own  specific  control  set.  However,  when  certain  structure  is  imposed  in  the  search  space,  a 
control  set  may  be  shared  by  some  of  the  vertices.  The  cardinality  of  a  control  set  of  a  vertex  is  its 
outdegree.  The  mean  of  outdegree  values  throughout  the  search  space  is  average  outdegree. 

Sampling  Concepts 

While  most  real-world  systems  are  continuous,  many  hard  problems  like  motion  planning  with 
differential  constraints  don’t  yet  have  exact  solutions  and  must  rely  on  approximation  to  be  able 
to  produce  a  solution  within  a  reasonable  amount  of  time.  Often,  there  is  a  trade-off  between 
the  quality  of  the  approximation  and  the  effort  it  takes  to  compute  the  solution.  Discretizing  the 
continuum  into  a  number  of  samples  is  one  way  to  manage  this  trade-off. 

Sampling  is  ubiquitous  in  many  areas  of  computation,  besides  motion  planning.  Most  planning 
algorithms  operate  by  sampling  the  search  space  of  the  problem,  e.g.  space  of  motions,  and  by 
evaluating  the  samples  while  looking  for  the  motion  that  optimizes  (1.2.8).  Two  basic  types  of 
sampling  are  randomized  and  deterministic.  The  former  relies  on  a  random  number  generator  to 
pick  samples.  The  latter  utilizes  a  certain  systematic  principle  in  making  sample  choices. 

A  large  body  of  work  has  been  dedicated  to  measuring  and  improving  the  quality  of  sampling. 
Some  popular  measures  of  sampling  quality  include  discrepancy  [101]  and  dispersion  [132].  One 
of  the  benefits  of  deterministic  sampling,  such  as  lattice  methods  [125],  is  the  relative  ease  of 
obtaining  good  sampling  quality.  One  potential  drawback,  however,  is  lack  of  flexibility,  without 
special  considerations,  to  vary  the  degree  of  sampling  in  some  parts  of  search  space,  e.g.  especially 
interesting  ones. 

The  forward  and  inverse  state  sampling  rule  s  :  — )■  N™  is  the  mapping  between  continuous 

and  discrete  state  values.  It  transforms  continuum  vehicle  states  xEX  into  discretized  values,  e.g. 
vertices  in  the  roadmap,  f  G  T'.  The  semantics  of  roadmap  edges  can  be  related  to  sampling  as 
well:  the  set  of  edges  “E  can  be  thought  of  as  a  sampled  subset  of  T.  In  the  sequel,  it  will  be  helpful 
to  use  the  sampled  version  of  state  X  and  trajectory  T  spaces;  the  symbols  refer  to  the  potentially 
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countably  infinite  subsets  of  the  respective  space: 

X  =  5(v),  Vv  G  X 

where  one  can  observe  that  elements  of  X  correspond  to  entire  subsets  of  X,  known  as  cells.  The 
value  X  then  serves  as  a  “representative”  value  of  the  cell.  E.g.,  if  a  cell  is  a  certain  ball  in  state 
space,  X  may  be  the  center  of  that  ball. 

A  sampled  version  of  the  cost  function  is  also  widely  used  in  robotics  and  is  known  as  a 
cost  map.  It  is  a  collection  of  state  space  cells  for  which  the  cost  function  is  evaluated  given  the 
representative  state  value  of  the  cell.  Evaluation  of  the  cost  of  the  trajectory  then  is  reduced  to  a 
Riemann  summation  operation  over  a  set  of  state  cells  that  are  occupied  by  the  trajectory  being 
evaluated.  The  set  of  such  cells  is  referred  to  as  a  swath  of  a  trajectory. 

1.2.2  Challenges 

This  thesis  identifies  four  specific  aspects  of  the  problem  of  motion  planning  with  differential  con¬ 
straints  that  account  for  its  complexity.  These  challenges  are  Feasibility,  Optimality,  Runtime  and 
Completeness.  They  are  further  described  below.  This  thesis  attempts  to  develop  search  spaces  that 
addresses  all  four  of  these  challenges.  Specific  measures  are  designed  (in  Chapter  5)  to  evaluate 
the  degree  to  which  these  challenges  are  met. 

Typically  trade-offs  exist  between  all  of  the  criteria  in  any  design  problem.  The  optimal  design 
problem  is  that  of  finding  the  best  design  given  some,  potentially  application-dependent,  weighted 
measures  of  them  all.  Eor  the  sake  of  generality,  no  optimal  design  will  be  attempted  here,  be¬ 
cause  the  basis  for  such  optimality  would  require  assuming  a  particular  application.  However,  the 
proposed  design  rules  will  at  least  be  somewhat  principled. 

Feasibility 

In  this  context,  feasibility  refers  to  the  challenge  of  solving  the  motion  planning  problem  for  a  con¬ 
trol  u{t)  that  satisfies  the  state  transition  equation  (1.2.1).  As  mobile  robots  continue  to  be  applied 
in  increasingly  difficult  environments,  travel  at  higher  speeds,  and  perform  more  complex  tasks, 
action  feasibility  of  motion  planner  solutions  becomes  very  important,  as  motivated  in  Section  1.1. 
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One  simple  counter-example  of  a  search  space  that  violates  the  feasibility  property  of  a  system 
in  Example  1.2.1  is  a  roadmap  with  “V  sampled  as  a  regular  grid  in  X  and  with  full  vertex  connec¬ 
tivity.  For  differentially  steered  mobile  robots  (or  others  which  can  execute  turn-in-place  actions), 
their  actions  can  be  modified  such  that  a  motion  plan  from  this  graph  is  followable,  but  it  would 
be  necessary  for  the  vehicle  to  reduce  its  velocity  to  zero  at  curvature  discontinuities  in  order  to 
follow  such  a  trajectory  closely.  If  reference  trajectory  deviation  greatly  increases  risk  of  failure,  a 
better  strategy  would  have  been  to  incorporate  feasibility  into  the  search  space  design. 

Feasibility  is  also  a  challenge  in  environments  where  terrain  variation  or  environment  effects 
(e.g.  currents,  loose  soil,  etc.)  are  significant.  If  the  roadmap  is  designed  without  taking  this 
variation  into  account,  some  edges  may  no  longer  satisfy  the  state  transition  equation  within  rea¬ 
sonable  bounds,  and  may  have  to  be  ignored  as  viable  motion  alternatives.  Unlike  kinematic  and 
dynamic  constraints,  feasibility  due  to  terrain  effects  and  environmental  effects  cannot  always  be 
determined  in  the  a  priori  roadmap  design. 

In  the  context  of  physical  systems,  feasibility  is  related  to  continuity  of  the  trajectory.  The 
continuity  is  a  matter  of  degree,  e.g.  C\  C^,  etc.  for  continuity  of  the  first  and  second  derivatives, 
respectively.  In  practice,  the  degree  of  continuity  is  often  chosen  based  on  the  application. 

Optimafity 

The  property  of  optimality  refers  to  the  quality  of  a  motion  solution.  Specifically,  it  refers  to  the 
minimization  in  (1.2.8).  As  suggested  in  the  definition  of  motion  cost,  often-used  forms  of  opti¬ 
mality  include  minimum  distance,  time,  energy,  risk,  although  generally  any  measure  of  interest 
could  be  used.  Two  different  forms  of  this  concept  are  local  and  global  optimality.  Global  opti¬ 
mality  is  achieved,  if  possible,  when  the  minimum-cost  solution  with  respect  to  T,  the  set  of  all 
feasible  motion  alternatives  that  steer  the  system  from  xi  to  xp,  is  determined.  Conversely,  local 
optimality  occurs  when  first  derivatives  of  the  cost  with  respect  to  the  parameters  that  define  the 
motion  are  zero  and  second  derivatives  are  positive. 

Runtime 

An  important  aspect  of  the  problem  being  considered  is  runtime,  the  clock  time  that  it  takes  to  com¬ 
pute  a  solution.  Computational  efficiency  is  of  key  concern  to  most  problems  in  computer  science. 
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and  it  is  especially  acute  in  robotics.  The  limits  of  acceptable  runtime  in  this  field  are  notably  lower 
than  in  many  other  domains.  A  motion  planner  and  any  other  algorithm  that  governs  the  action  of 
a  physical  entity  in  motion,  must  be  able  to  react  quickly  enough  so  as  to  avoid  potential  damage 
due  to  a  collision  or  other  undesirable  interaction  with  environment.  Many  classical  approaches 
to  motion  planning  with  differential  constraints  do  not  place  sufficient  emphasis  on  the  algorithm 
runtime  in  real  terms.  One  of  the  discriminators  of  this  thesis  is  great  concern  with  guarantees  on 
planner  runtime,  and  the  aim  to  validate  the  proposed  approaches  on  real  platforms. 

Completeness 

Motion  planning  completeness  is  the  property  of  a  planning  algorithm  of  being  able  to  determine 
whether  an  action  that  satisfies  the  motion  planning  problem  can  be  found,  or  determined  not  to 
exist.  In  planning  for  nontrivial  mobile  robot  systems  (e.g.  with  differential  constraints,  espe¬ 
cially  those  that  operate  in  obstacle-ridden  environments)  is,  however,  rarely  guaranteed.  Related 
concepts  include  resolution-completeness,  an  ability  of  the  planner  to  satisfy  the  completeness 
property  in  the  limit  as  the  fidelity  of  its  representation  is  increasingly  improved  [79],  and  prob¬ 
abilistic  completeness,  a  property  of  the  planner  to  be  able  to  find  a  solution  with  probability  of 
unity  in  the  limit  as  its  search  space  sampling  density  continues  to  increase  [87]. 

1.2.3  Scope 

This  thesis  makes  several  assumptions  to  define  the  scope  of  the  research: 

•  A  motion  model  (1.2.1)  is  known,  although  it  is  assumed  to  be  inaccurate.  This  work  does 
not  consider  the  problem  of  discovering  the  model,  e.g.  system  identification. 

•  Design  of  immediate,  local  reactive  controllers  is  beyond  the  scope  of  this  work.  An  attempt 
is  made  to  avoid  intersecting  with  this  separate,  yet  related  field.  It  is  assumed  that  such 
a  controller  is  available  for  the  system,  and  it  is  capable  of  solving  a  two-point  boundary 
value  problem  for  the  system,  as  illustrated  in  Figure  1.2.2.  It  is  also  assumed  that  benign, 
unmodelled  environment  variations  (e.g.  terrain  shape,  loose  soil,  currents,  etc.)  and  other 
perturbations  are  eliminated  by  this  controller. 
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•  The  controllability  properties  of  the  system  allow  a  finite  subset  of  its  reachability  tree  to  be 
an  accurate  representation  of  the  system’s  maneuverability.  The  degree  to  which  this  repre¬ 
sentation  is  an  accurate  one  will  influence  the  quality  of  the  search  spaces  being  designed 
with  the  proposed  methods. 

•  The  finite  representation  of  system  reachability  verifies  translational  symmetry:  the  reach¬ 
ability  is  invariant  to  the  initial  position  of  the  robot  in  its  environment.  While  a  simulated 
robot  operating  on  a  flat  plane  would  easily  satisfy  this  assumption,  real  physical  robots 
typically  rely  on  low-level  controllers  to  eliminate  the  inevitable  perturbations  that  they  ex¬ 
perience  while  traversing  their  environment.  This  assumption  is  typically  made  in  motion 
planning  in  order  to  alleviate  the  additional  computational  complexity  of  explicitly  reasoning 
about  these  perturbations  (both  known  and  discovered  during  motion). 

•  Explicit  reasoning  about  the  uncertainty  in  the  environment  is  not  a  specific  requirement 
for  building  a  functional  and  effective  motion  planner.  Uncertainty  of  the  robot  and  its 
environment  is  accounted  for  implicitly,  however.  The  approach  is  nevertheless  validated 
with  substantial  experimentation  with  physical  autonomous  robots  in  realistic  settings. 

1.3  Approach 

The  approach  presented  in  this  thesis  builds  upon  a  long  heritage  and  many  strengths  of  earlier 
motion  planning  techniques.  In  particular,  the  method  builds  upon  the  previous  use  of  an  implicit 
representation  of  the  search  space  [21],  the  research  in  BVP  solver  and  controller  design  ([53;  123] 
among  others),  incremental  deterministic  sampling  [85],  and  an  off-line  analysis  of  the  reachability 
set  of  the  system  for  improved  on-line  performance  [43]. 

In  general  terms,  the  crux  of  the  proposal  is  the  generation  of  a  search  space  -  a  roadmap, 
represented  as  a  directed  cyclic  graph  -  that  satisfies  differential  constraints  by  construction.  The 
problem  of  motion  planning  is  thereby  reduced  to  unconstrained  heuristic  search  in  a  search  space 
of  specific  structure  and  appropriate  dimensionality.  This  setup  leads  to  a  number  of  efficiencies 
that  are  critical  to  robotics  applications,  especially  those  facing  frequent  and  unpredictable  changes 
in  the  environment  model,  e.g.  due  to  sensor  uncertainty  or  other  effects.  The  proposed  structure 
of  motion  primitives  leads  to  the  ability  to  generate  sets  of  such  motion  primitives  automatically. 
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1.4  Contribution 

The  specific  contributions  of  this  thesis  are  as  follows: 

•  A  new  method  for  motion  planning  with  differential  constraints  based  on  deterministic  sam¬ 
pling.  The  method  is  demonstrated,  in  experiments  with  real  robots,  to  offer  unprecedented 
performance  (with  respect  to  the  criteria  in  Section  1.2.2)  in  robotics  applications  with  fre¬ 
quent  and  unpredictable  changes  in  the  environment  model. 

•  A  new  type  of  motion  primitives  that  possess  the  necessary  structure  to  result  in  search 
spaces  with  cyclic  graph  topology,  which  offers  a  number  of  advantages  over  search  spaces 
that  are  standard  in  this  domain. 

•  The  first  method  of  automatic  generation  of  motion  primitives,  generally  applicable  to  a  large 
class  of  systems  with  differential  constraints,  based  solely  on  the  given  system  transition 
model. 

1.5  Applications 

The  contributions  of  this  thesis  are  relevant  to  any  system  featuring  differential  constraints,  op¬ 
erating  in  a  complex,  unknown  operational  environment.  A  number  of  current  mobile  robot  ap¬ 
plications  are  of  particular  interest,  including  field  robotics,  exploration  robotics,  agricultural  and 
mining  systems,  autonomous  automobiles,  and  mobile  manipulators. 

1.5.1  Off-road  Navigation 

The  initial  motivation  for  this  work  came  from  experiences  with  car-like  mobile  robots  operating 
in  cluttered  natural  terrain.  Unstructured  environments  often  pose  significant  challenges  to  mobile 
robots.  The  perception  problem  is  known  to  be  difficult;  however,  even  if  detailed,  timely,  and 
accurate  perception  information  is  available,  it  is  still  a  difficult  problem  to  fully  utilize  it  in  mo¬ 
tion  planning,  especially  under  real-time  constraints.  Today’s  navigation  algorithms  often  make 
mistakes  that  result  in  the  vehicle  being  trapped  in  a  set  of  dense  obstacles  with  no  way  to  proceed 
in  the  forward  direction. 
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During  the  DARPA  Perception  for  Off-Road  Robotics  (PerceptOR)  project,  our  vehicle  faced 
such  problems  several  times  an  hour  in  challenging  terrains.  The  initial  ideas  of  the  approach 
presented  herein  were  investigated  first  on  this  program.  An  implementation  was  developed  using 
most  of  the  central  concepts  of  this  method:  an  explicit  state  lattice  based  on  the  mobility  model 
of  the  PerceptOR  vehicles  (Figure  1.5.1)  was  precomputed,  and  the  A*  algorithm  was  utilized  to 
search  it  [66].  The  algorithm  was  invoked  as  an  aggressive  limited  scope  behavior  when  simpler 
alternatives  failed  to  produce  a  solution. 


Figure  1.5.1:  PerceptOR  robot.  A  preliminary  version  of  our  results  was  verified  on  PerceptOR 
vehicles,  designed  to  navigate  in  natural  environments. 

Note  that  the  final  formulation  of  the  method  presented  here  achieves  about  two  orders  of 
magnitude  speed  up  relative  to  the  one  utilized  on  PerceptOR.  Custom,  obstacle  avoiding  n-point 
turn  maneuvers  can  now  be  generated  in  reactive  fashion.  The  extended  horizon  of  the  planner 
would  enable  the  generation  of  a  backup  maneuver  all  the  way  out  of  a  discovered  cul-de-sac,  if 
this  maneuver  was  considered  preferable  to  a  complicated  n-point  turn. 

1.5.2  Planetary  Rover  Locomotion 

Motion  planning  for  planetary  exploration  rovers  is  in  many  ways  similar  to  the  terrestrial  problem 
discussed  above.  However,  it  is  also  has  many  distinctive  features.  One  significant  difference 
is  that  rovers  typically  move  fairly  slowly,  only  several  centimeters  per  second,  for  a  variety  of 
reasons  including  stringent  weight,  computational  and  power  limitations.  Moreover,  the  rovers  are 
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typically  designed  to  move  in  stop  and  go  fashion.  GESTALT,  the  rover  navigation  algorithm  that 
drives  the  MER  rovers  uses  higher  level  pereeption,  sueh  as  stereo  vision,  while  it  is  stationary 
and  seleets  the  best  are  path  to  follow.  Afterwards,  the  rover  eommits  to  driving  the  seleeted  are 
without  pereeption,  using  only  lower  level  sensors  sueh  as  eurrent  sensors  [47]. 

Despite  these  differenees,  the  the  solution  presented  herein  ean  be  applied  to  the  rover  loeomo- 
tion  problem  in  a  straight-forward  manner.  One  way  to  apply  it  is  similar  to  the  terrestrial  ease,  i.e. 
as  a  baekup  behavior  that  is  invoked  only  when  the  rover  is  no  longer  able  to  navigate  further  due 
to  errors  in  pereeption,  path  following,  state  estimation,  ete.  In  this  ease  the  rover  would  eompute 
a  reseuing  path  and  get  baek  on  traek  autonomously.  Euture  adoption  of  teehnologies  sueh  as  this 
will  undoubtedly  reduee  the  effort  and  eost  of  manual  rover  path  planning. 

The  presented  planner  may  also  be  applied  in  other  ways.  Eor  example,  given  its  effieieney, 
it’s  reasonable  to  imagine  that  navigation  algorithms  sueh  as  GESTALT  eould  be  enhaneed  with 
lattiee  planning  eapability,  sueh  that  instead  of  are  seleetion,  they  might  perform  lattiee  seareh 
and  produee  the  eomplete  path:  not  only  an  are,  but  an  ”S”  shape  or  anything  else  as  required  by 
the  environment.  Given  the  effieieney  of  the  planner,  the  eomputation  required  is  promising  to  be 
eomparable  to  the  eonventional  method  of  evaluating  the  requisite  number  of  ares  and  running  an 
arbiter. 


Eigure  1.5.2:  FIDO  rover.  Rover  is  moving  autonomously  under  the  guidanee  of  the  state  lattiee 
planner,  implemented  within  the  CLARAty  software  arehiteeture. 


As  was  diseussed  above,  in  simple  to  medium  obstaele  fields,  the  lattiee  planner  eompares  in 
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efficiency  to  grid  search,  primarily  because  the  heuristic  we  designed  accurately  guides  the  search 
such  that  backtracking  is  minimized.  The  search  can  be  visualized  as  proceeding  in  depth-first 
manner,  but  leading  invariably  towards  the  goal,  thanks  to  the  ideal  heuristic  encoded  in  the  look¬ 
up  table.  In  many  implementations,  obstacle  avoidance  arc  evaluation  is  performed  like  breadth- 
first  search  to  unity  depth:  many  arcs  are  evaluated,  but  only  is  ultimately  used.  With  careful 
design,  the  presented  planner  could  save  computation  and  extend  the  planning  horizon  to  the  limits 
of  perception. 

An  implementation  of  the  state  lattice  planner  has  been  integrated  with  the  Coupled  Layer 
Architecture  for  Robotic  Autonomy  (CLARAty)  system  at  the  Jet  Propulsion  Laboratory.  This  al¬ 
lowed  us  to  test  the  application  of  the  planner  to  rover  locomotion  through  simulation  using  JPL’s 
high-fidelity  rover  dynamics  simulation  system,  ROAMS  (Rover  Modeling  and  Simulation).  Fig¬ 
ure  1.5.2  depicts  an  experiment  with  the  FIDO  research  prototype  rover  performing  autonomous 
navigation  in  rough  terrain.  The  goal  is  to  move  toward  the  goal  in  a  field  spotted  with  large  ob¬ 
stacles.  An  important  motivation  for  this  work  is  to  assist  in  the  single-cycle  instrument  placement 
task.  Currently  it  takes  three  to  four  Martian  days  for  rover  drivers  to  move  the  rover  to  the  desired 
science  target.  By  using  this  planner,  we  believe  the  instrument  placement  task  will  be  simplified 
by  making  the  navigation  efficient  computationally,  by  fully  utilizing  high-level  perception  infor¬ 
mation  by  computing  as  long  ’’blind  drive”  paths  as  perception  allows,  and  by  guaranteeing  that  the 
rover’s  navigator  never  gets  stuck,  as  unlimited  search  will  always  find  the  path  out  of  cul-de-sacs, 
whereas  typical  arc-based  navigators  typically  are  not  designed  for  this  purpose. 

1.5.3  Reactive  Motion  Peanning  and  Path  Modieication 

By  virtue  of  its  efficiency,  the  presented  planner  can  be  useful  in  a  variety  of  other  tasks.  In  partic¬ 
ular,  it  could  be  applied  to  automatic  mobile  equipment  that  performs  routine  and  repetitive  tasks, 
e.g.  earth  movers,  harbor  transporters,  convoy  vehicles,  etc.  It  is  worth  noting  that  such  systems 
can  benefit  from  more  structured  environments  than  unknown  natural  terrain.  It  is  possible  to  build 
a  reactive  motion  planning  system  that  will  be  able  to  modify  its  current  path  immediately  upon 
perceiving  an  unexpected  obstacle.  Moreover,  as  we  have  shown  in  the  discussion  of  admissibility 
of  the  heuristic,  the  new  path  will  be  optimal,  which  is  likely  to  result  in  minimum  deviation  from 
the  original  course,  just  enough  to  negotiate  the  unexpected  obstacle. 
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1.5.4  Mining  and  Agricultural  Robotics 

Resource  extraction  automation  will  revolutionize  the  mining  and  agriculture  industries  in  the  next 
century.  Autonomous  vehicles  will  lead  to  more  stable,  efficient,  and  cost  effective  platform  de¬ 
sign.  Resources  could  be  harvested  in  dangerous,  remote  environments  including  steep  mountain¬ 
side  terraces,  the  ocean  floor,  asteroids,  and  the  Moon.  Mobile  robots  that  better  follow  natural 
resource  contours  makes  autonomous  systems  more  profitable.  Autonomous  agricultural  systems 
that  generate  minimum-length  motion  plans,  satisfy  mobility  and  environmental  constraints,  and 
extract  resources  at  a  faster  rate  can  decrease  overhead  costs  by  reducing  fuel  consumption  and 
maintenance.  Reasoning  about  the  kinematic  constraints  of  the  vehicle  is  important  when  the  turn¬ 
ing  radius  may  be  limited,  since  the  vehicle  may  have  to  initially  turn  away  from  the  intended  target 
to  achieve  the  desired  state.  Likewise,  autonomous  agricultural  and  mining  systems  often  repeat 
similar  motion  plans  in  the  same  environment  many  times.  By  progressively  adapting  the  search 
space  to  conform  exactly  to  the  particular  environment,  the  optimality  of  the  resulting  motion  plans 
is  likely  to  increase,  resulting  in  potentially  faster,  better,  and  cheaper  systems. 

1.5.5  Autonomous  Automobiles 

The  automation  of  the  streets  and  highways  in  modern  cities  using  roboticized  automobiles  will 
lead  to  safer  and  more  efficient  transportation  systems  for  humans.  To  achieve  this,  developments 
in  perception,  localization,  mapping,  communication,  motion  planning,  and  control  must  occur. 
Motion  plan  feasibility  is  important  at  the  speeds  associated  with  highway  navigation.  Reasoning 
about  kinematic  and  dynamic  constraints  while  performing  swerves,  lane  changes,  or  simple  lane 
following  is  necessary  to  maintain  vehicle  safety.  Planning  infeasible  actions  and  relying  on  lower- 
level  path  followers  and  obstacle  avoidance  algorithms  can  be  disastrous  if  the  difference  between 
the  planned  and  actual  actions  is  significant. 

1.6  Organization 

This  dissertation  is  organized  in  9  chapters.  First,  terminology,  concepts,  and  challenges  addressed 
in  this  thesis  are  described  in  Chapter  1.  Related  work  in  mobile  robot  motion  planning,  naviga¬ 
tion,  and  control  follow  this  discussion  in  Chapter  2.  In  Chapter  3  we  embark  from  our  problem 
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definition  and  construct  the  proposed  type  of  motion  primitives,  followed  by  a  discussion  of  their 
features  and  requirements.  The  following  Chapter  4  is  dedicated  to  the  details  of  exploiting  the 
properties  of  these  primitives  for  building  an  efficient  motion  planners  using  a  variety  of  search 
algorithms.  Chapter  5  embarks  on  a  discussion  of  automatic  design  of  state  lattice  motion  prim¬ 
itives.  It  suggests  quality  metrics  for  such  search  spaces,  so  that  alternative  search  space  designs 
may  be  compared  in  order  to  select  the  design  of  better  quality.  Chapter  6  provides  recommen¬ 
dations  for  developing  state  space  sampling  for  improved  performance  of  the  resulting  planner. 
Finally,  Chapter  7  presents  a  set  of  algorithms  for  the  automatic  generation  of  the  given  type  of 
motion  primitives.  A  thorough  account  of  experimental  validation  of  the  key  results  in  this  thesis 
are  given  in  Chapter  8.  The  thesis  concludes  with  a  summary  and  a  review  of  the  contributions  in 
Chapter  9. 

1.7  Publication  Note 

Portions  of  the  presented  motion  planning  research  in  Chapters  3  and  4  appear  in  [109;  110]. 
The  related  field  experiments  are  discussed  in  [113].  Extensions  of  this  work  for  dynamic  search 
spaces  appear  in  [1 1 1;  114].  Approaches  for  automatic  generation  of  motion  primitives  were  given 
in  [112]. 
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RELATED  WORK 


This  thesis  is  concerned  with  developing  a  novel  approach  to  motion  planning  with  differential 
constraints  that  lends  itself  well  to  application  on  physical  robots  operating  in  difficult,  challeng¬ 
ing  environments.  Substantial  prior  research  has  advanced  mobile  robot  motion  planning  and 
navigation  to  the  point  where  mobile  robots  can  successfully  traverse  open  deserts,  navigate  urban 
environments,  and  explore  the  oceans  and  other  bodies  in  our  solar  system.  This  section  reviews 
prior  work  in  mobile  robot  motion  planning,  and  navigation  related  to  the  topics  of  this  thesis. 

2.1  Vehicle  Constraints 

A  significant  amount  of  work  has  been  dedicated  in  recent  years  to  the  problem  of  smooth  in¬ 
verse  trajectory  generation  for  nonholonomic  vehicles:  finding  a  smooth  and  feasible  path  given 
two  end-point  configurations.  While  in  general  this  is  a  difficult  problem,  recent  progress  in  this 
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area  produced  a  variety  of  fast  algorithms.  The  ground-breaking  work  in  analyzing  the  paths  for 
nonholonomic  vehicles  was  done  by  Dubins  [30]  and  Reeds  and  Shepp  [115].  Their  ideas  were 
further  developed  in  algorithms  proposed  by  Scheuer  and  Laugier  [123],  Fraichard  and  Ahuactzin 
[40],  and  Fraichard  and  Scheuer  [41]  where  smoothness  of  paths  was  achieved  by  introducing  seg¬ 
ments  of  clothoids  (curves  whose  curvature  is  a  linear  function  of  their  length)  along  with  arcs 
and  straight  line  segments.  Somewhat  different  approaches  by  Scheuer  and  Fraichard  [122],  and 
Lamiraux  and  Laumond  [78],  among  others,  have  also  been  shown  to  solve  the  generation  prob¬ 
lem  successfully  and  quite  efficiently.  On  the  other  hand,  Frazzoli,  Dahleh  and  Feron  [44]  suggest 
that  there  are  many  cases  where  efficient,  obstacle-free  paths  may  be  computed  analytically,  e.g. 
the  systems  with  linear  dynamics  and  a  quadratic  cost  (double  or  triple  integrators  with  control 
amplitude  and  rate  limits).  The  cases  that  do  not  admit  closed-form  solutions  can  be  approached 
numerically  by  solving  appropriate  optimal  control  problems  (e.g.  [37],  [6]).  A  fast  nonholonomic 
trajectory  generator  by  Kelly  and  Nagy  [63],  Howard  and  Kelly  [52]  generates  polynomial  spiral 
trajectories  using  parametric  optimal  control. 

2.2  Environment  Constraints 

Some  of  the  methods  described  above  also  proposed  applications  to  planning  among  obstacles. 
Since  the  beginning  of  modem  motion  planning  research  ([94],  [93],  [116]),  there  has  been  in¬ 
terest  in  the  planning  methods  that  constructed  boundary  representations  of  configuration  space 
obstacles  ([23],  [2],  [1]  and  others).  The  complexity  of  motion  planning  algorithms  has  also  been 
studied  ([24],  [100],  [3],  [59]).  With  the  advent  of  efficient  C-space  sampling  methods  ([10],  [48]), 
there  has  been  interest  in  algorithms  that  sample  the  space  in  deterministic  fashion  ([11],  [9],  others 
in  [79]).  Lacaze  et  al.  [77]  utilized  these  ideas  to  propose  a  method  for  planning  over  rough  ter¬ 
rain  using  generation  of  motion  primitives  by  integrating  the  forward  model.  Cherif  [27]  advanced 
these  concepts  by  basing  planning  on  physical  modeling.  Note  that  one  of  principal  differences 
(and  a  novelty,  to  our  knowledge)  of  our  approach  is  leveraging  the  research  in  inverse  trajectory 
generators  to  generate  motion  primitives  under  convenient  discretization.  Smoothing  with  primi¬ 
tives  was  also  evaluated  [39]. 
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2.3  Randomized  Motion  Planning 

Also  in  the  early  1990s,  randomized  sampling  was  introduced  to  motion  planning  ([8],  among 
others).  The  Probabilistic  Roadmap  (PRM)  methods  were  shown  to  be  well-suited  for  path  plan¬ 
ning  in  C-spaces  with  many  degrees  of  freedom  ([62],  [61],  [54]),  and  with  complex  constraints, 
e.g.  nonholonomic,  kinodynamic,  etc.  ([74],  [54],  [25],  [68]).  Another  type  of  probabilistic  plan¬ 
ning  was  Rapidly-exploring  Random  Trees  (RRT)  introduced  by  LaValle  and  Kuffner  ([82], [83]). 
RRTs  were  originally  developed  for  handling  differential  constraints,  although  they  have  also  been 
widely  applied  to  the  Piano  Mover’s  problem  ([86]).  Randomized  approaches  are  understood  to  be 
incomplete,  strictly  speaking,  but  capable  of  solving  many  challenging  problems  quite  efficiently 
([21]). 

As  the  randomized  planners  became  increasingly  well  understood  in  recent  years,  it  was  sug¬ 
gested  that  their  efficiency  was  not  due  to  randomization  itself.  LaValle,  Branicky  and  Lindemann 
[85]  suggest  an  intuition  that  real  random  number  generators  always  have  a  degree  of  determinism. 
In  fact,  Branicky  et  al.  [21]  show  that  quasi-random  sampling  sequences  can  accomplish  simi¬ 
lar  or  better  performance  than  their  random  counterparts.  The  improvements  in  performance  are 
primarily  attributed  to  the  more  uniform  sampling  of  quasi-random  methods.  For  these  reasons, 
Branicky  et  al.  [21]  introduced  Quasi-PRM  and  Lattice  Roadmap  (LRM)  algorithms  that  used  low- 
discrepancy  Halton/Hammersley  sequences  and  a  regular  lattice,  respectively,  for  sampling.  Both 
methods  were  shown  to  be  resolution-complete,  while  the  LRM  appeared  especially  attractive  due 
to  its  properties  of  optimal  dispersion  and  near-optimal  discrepancy.  In  this  light,  our  approach  of 
sampling  on  a  regular  lattice  can  be  considered  to  be  one  of  building  on  the  LRM  idea  and  making 
it  more  efficient  through  a  detailed  analysis  of  a  fit  between  the  reachability  graph  of  the  system 
and  the  underlying  sampling  lattice. 

2.4  Implicit  Sampling 

Recent  works  have  also  discussed  ’’lazy”  variants  of  the  above  planning  methods  that  avoid  colli¬ 
sion  checking  during  the  roadmap  construction  phase  (e.g.  [19],  [17],  [21],  [120],  [121]).  In  this 
manner  the  same  roadmap  could  be  used  in  a  variety  of  settings,  at  the  cost  of  performing  colli¬ 
sion  checking  during  the  search.  An  even  ’’lazier”  version  is  suggested,  in  which  ’’the  initial  graph 
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is  not  even  explieitly  represented”  [21].  In  this  regard,  our  approaeh  of  using  an  implieit  lattiee 
and  searehing  it  by  means  of  a  pre-eomputed  eontrol  set  that  only  eaptures  loeal  eonneetivity  is 
similar  to  the  Lazy  LRM.  Our  eontribution  is  in  exploring  the  eonjeeture  made  in  that  work  and 
sueeessfully  applying  it  to  nonholonomie  motion  planning. 

2.5  Sampling 

The  question  of  lattiee  sampling  has  also  been  raised  in  the  eontext  of  motion  planning  utilizing 
eontrol  quantization.  Bieehi,  Marigo  and  Piecoli  [16]  as  well  as  Paneanti,  Pallottino  and  Bieehi 
[106]  showed  that,  for  systems  that  ean  be  expressed  as  q  =  g{q,u)  that  are  not  under-aetuated, 
through  eareful  diseretization  in  eontrol  spaee,  it  is  possible  to  foree  the  resulting  reaehability 
graph  of  the  system  to  be  a  lattiee.  It  was  also  shown  that  this  teehnique  ean  be  applied  to  a 
large  elass  of  nonholonomie  systems.  That  approaeh  presents  a  way  to  generate  a  path  given  its 
terminal  points  and  shows  how  under  suitable  eonditions  a  regular  lattice  of  reachable  points  can 
be  achieved.  However,  this  is  usually  difficult  to  achieve,  and  under  most  quantizations  the  vertices 
of  the  reachability  graph  are  unfortunately  dense  in  the  reachable  set  [86].  By  contrast,  our  method 
uses  an  inverse  path  generator  capable  of  generating  essentially  any  feasible  motion,  so  we  can 
choose  a  convenient  discretization  of  state  space  based  on  the  problem  demands  and  generate  a 
custom  set  of  controls  to  suit  this  discretization. 

In  this  work  we  also  build  on  a  considerable  amount  of  research  in  analysis  of  system  reacha¬ 
bility.  One  line  of  research  uses  planning  methods  themselves  to  analyze  reachability  of  complex 
systems  [15].  Since  we  focus  on  applications  of  motion  planning  of  wheeled  vehicles,  we  have 
developed  a  technique  based  on  exhaustive  search  that  has  worked  quite  well  for  exploration  of 
the  reachability  graph.  Our  method  involves  pruning  this  search  early  to  analyze  reachability  effi¬ 
ciently. 

2.6  Motion  Primitives 

There  has  been  significant  interest  recently  in  developing  motion  primitives,  specially  designed 
controls  that  facilitate  motion  planning,  particularly  under  kinematics  and  dynamics  constraints  on 
robot  motion.  The  controls  are  developed  during  construction  of  the  planner  (pre-computed)  and 
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represent motions,  i.e.  those  that  satisfy  the  constraints  of  the  system.  Motion  planners 
can  utilize  these  primitives  to  enact  efficient  search  in  state  space  by  ignoring  system  constraints, 
instead  focusing  on  the  environment  and  other  constraints  -  thereby  improving  efficiency  of  the 
planning.  The  role  of  primitives  in  planning  and  the  importance  of  their  quality  have  been  moti¬ 
vated  both  in  deterministic  [32;  46;  90]  and  randomized  [44]  planning  domains.  Their  importance 
was  also  noted  in  the  related  area  of  reactive  obstacle  avoidance  in  the  context  of  mobile  robot 
navigation  [20;  49;  80].  A  number  of  popular  approaches  to  kinematic  and  kinodynamic  planning 
can  readily  incorporate  primitives  in  their  design.  The  requisite  local  planner  in  [55;  61;  87]  can 
be  implemented  as  a  process  that  chooses  an  appropriate  element  from  a  set  of  primitives  [44].  In 
deterministic  approaches  [11;  29;  34;  46;  60;  90],  the  vertex  expansion  (set  of  edges  emanating 
from  a  vertex)  can  be  a  pre-determined  set  of  primitives  based  on  the  state  value  that  the  vertex 
represents.  Dynamic  path  modification  was  done  in  [67]. 

Motion  primitives  have  been  designed  in  the  past  through  sampling  control  space  in  such  a  way 
as  to  result  in  good  sampling  in  state  space  in  terms  of  discrepancy,  dispersion  or  path  diversity 
[20;  32;  46;  49;  80;  107].  We  refer  to  this  line  of  work  as  control- sampling  primitives.  In  general, 
designing  such  primitives  is  difficult  due  to  the  complexity  of  the  relationship  between  the  robot’s 
control  and  state  spaces  under  kinematics  and  dynamics  constraints.  Motivated  by  this,  we  propose 
state  lattice  primitives,  designed  via  a  reverse  process.  First,  we  establish  an  attractive  sampling 
rule  in  state  space,  perhaps  one  that  is  convenient  and  efficient  for  the  planning  problem  (e.g. 
commensurate  with  the  robot’s  world  model,  such  as  an  occupancy  grid).  Then,  we  compute  the 
controls  that  steer  the  system  between  these  samples  using  a  boundary  value  problem  (B VP)  solver. 
The  approach  can  be  viewed  as  a  way  of  extending  the  Lazy  LRM  [88]  to  handle  kinematics  and 
dynamics  constraints  by  leveraging  the  related  research  in  BVP.  Such  solvers  are  available  for  a 
variety  of  systems,  such  as  car-like  [115],  chained  form  [98;  107],  as  well  as  in  rough  terrain  [53] 
and  dynamics  [72;  131]  settings. 
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MOTION  PRIMITIVES 


This  section  presents  a  progression  of  design  principles  that  results  in  the  creation  of  a  search  space 
that  is  the  topic  of  this  work  and  that  was  introduced  in  Section  1.3. 

3.1  Regular  Lattices  and  Grids 

In  problems  where  analytic  representations  are  not  convenient,  a  useful  step  in  problem  formulation 
is  to  establish  a  sampling  policy  in  order  to  avoid  attempting  to  search  the  entire  continuum.  As 
suggested  in  Section  1.2,  beneficial  sampling  policies  include  those  that  cover  a  larger  volume 
of  the  state  space  with  fewer  samples.  It  is  natural  to  extend  this  concept  from  values  of  state 
space  to  paths  to  functions,  elements  of  T.  Similar  to  the  state  space,  the  continuum  of  motions 
that  are  executable  by  the  system  can  also  be  sampled  to  make  computation  tractable.  This  thesis 
attempts  to  develop  a  sampling  of  motions  that  is  both  convenient  to  design  and  effective  for  motion 
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planning. 

Although  there  are  alternatives  of  a  probabilistie  nature,  this  work  explores  seareh  strueture 
afforded  by  deterministie  sampling  meehanisms  whieh  produee  regular  lattiees  of  states.  A  prinei- 
pal  advantage  of  regular  sampling  is  translational  invarianee.  Any  eontrol  that  joins  two  different 
states  will  also  join  all  other  pairs  of  identieally  arranged  states.  Through  an  applieation  of  the 
same  argument  to  all  paths  joining  any  vertex  of  a  roadmap  to  its  neighbors,  it  beeomes  elear  that 
the  same  set  of  eontrols  emanating  in  a  repeating  pattern  from  a  given  state  ean  be  applied  at  every 
other  instanee  of  the  repeating  unit.  Therefore,  in  this  ease  of  a  regular  lattiee,  the  information 
eneoding  the  eonneetivity  of  the  seareh  spaee  (ignoring  obstacles)  can  be  precomputed,  and  it  can 
be  stored  compactly  in  terms  of  a  canonical  set  of  repeated  primitive  motions,  the  control  set  of  the 
lattice. 


Figure  3.1.1:  Regular  Lattices.  Top:  rectangular,  diamond,  and  triangular  (hexagonal)  lattices. 
Bottom  Left:  Controls  for  a  4-connected  lattice.  Bottom  Right:  Discontinuous  heading  change. 

The  simplest  case  of  a  regular  lattice  is  a  rectangular  grid  (Figure  3.1.1).  In  this  case,  it  is 
common  to  use  a  control  set  consisting  of  North,  South,  East,  and  West  motions  to  create  a  “4- 
connected”  graph.  While  this  is  straightforward,  the  transitions  between  controls  that  occur  at  the 
vertices  can  be  problematic  in  practice  due  to  discontinuity  in  some  of  the  state  dimensions.  Con¬ 
sider,  for  example,  a  path  which  enters  a  vertex  from  the  south  and  then  leaves  toward  the  east. 
When  heading  changes  across  a  vertex  in  a  generated  path,  its  execution  at  any  nonzero  linear 
velocity  implies  an  instantaneous  (and  therefore  impossible)  heading  change.  Similarly,  even  a 
transition  between  two  different  path  curvatures  at  the  same  heading  is  infeasible  at  nonzero  ve- 
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locity;  discontinuity  in  time  derivatives,  such  as  velocity  and  acceleration,  are  equally  undesirable. 
The  elimination  of  such  infeasible  plans  requires  a  mechanism  to  enforce  continuity  at  the  vertices. 

3.2  State  Lattice  Motion  Primitives 

Assuming  lattice  state  discretization  outlined  earlier,  motion  primitives  are  defined  here  to  be  the 
controls  that  connect  roadmap  vertices  (states)  and  that  are  feasible  motions.  Further,  enforcing 
state  continuity  requires  that  the  state  dimension  vector  be  augmented  with  appropriate  dimension¬ 
ality.  A  simple  example  of  such  a  search  space  is  the  state  lattice  for  a  Reeds-Shepp  car  [115] 
in  Figure  3.2.1.  This  system  has  the  same  kinematics  as  the  one  in  Example  1.2.1,  but  with  a 
restriction  that  its  steering  angle  can  have  only  three  values,  [— (|)max,0,<limax]^,  where  (|)max  is  the 
maximum  steering  angle  of  the  robot.  A  slightly  more  involved  example  of  a  state  lattice  for  a 
car-like  robot  without  such  a  restriction  is  shown  in  Figure  3.2.2. 


Figure  3.2.1:  State  lattice  for  Reeds-Shepp  car.  The  Reeds-Shepp  car  can  move  forward  and 
backward  and  it  can  drive  straight  or  turn  left  or  right  at  a  fixed  curvature.  The  presented  rudi¬ 
mentary  control  set  is  derived  from  these  basic  controls  by  choosing  their  length  such  that  only 
four  heading  values  are  used.  Left:  The  designed  control  set  precisely  hits  vertices  in  a  rectangular 
grid.  Center:  The  reachability  tree  to  depth  2.  Right:  The  reachability  tree  obtained  by  copying 
the  control  set  at  every  vertex  in  a  C  space  with  4  headings.  Each  dot  represents  4  distinct  vertices 
overlaid  over  each  other,  each  representing  different  values  of  heading.  While  this  search  space 
will  not  generate  a  turn  exceeding  the  chosen  curvature,  and  while  heading  is  continuous  across 
vertices,  the  instantaneous  transitions  among  curvatures  at  the  vertices  do  not  respect  steering  rate 
limitations. 

By  following  these  principles,  we  arrive  at  the  construct,  referred  to  as  a  state  lattice,  a  roadmap 
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that  consists  of  motion  primitives  developed  in  such  a  way  as  to  connect  the  vertiees  of  the 
roadmap,  sampled  as  a  lattiee  in  state  spaee.  By  sampling  regularly,  the  eonneetivity  of  this 
roadmap  is  eompletely  speeified  with  its  eontrol  set.  Any  systematie  graph  seareh  algorithm  ean 
be  utilized  to  find  the  shortest  path  in  the  state  lattiee  graph. 


Figure  3.2.2:  State  lattice  for  general  car-like  system.  This  is  one  of  the  potential  designs  of  a 
state  lattiee  for  a  ear-like  system  in  Example  1.2.1.  The  motion  primitives  are  regularly  arranged 
in  state  spaee;  to  design  them,  a  eonvenient  state  sampling  rule  is  ehosen  (e.g.  a  low  diserepaney 
lattiee),  and  a  eontroller  is  used  to  eonneet  the  samples  via  feasible  motions.  Given  heading  dis- 
eretization  in  this  partieular  example,  sueh  a  set  is  to  be  defined  for  eaeh  of  the  8  diserete  values 
of  initial  heading,  the  multiples  of  7r/4;  symmetry  ean  be  exploited  to  define  only  two  sueh  sets, 
eorresponding  to  the  initial  heading  in  the  interval  [0,7r/2). 

The  process  of  eonstrueting  the  state  lattiee  assumes  a  eapability  to  determine  a  eontrol  to 
the  system  that  steers  it  from  one  state  to  another.  In  general,  finding  this  eontrol  is  identieal  to 
solving  a  BVP;  exaet  solvers  are  available  only  for  simple  systems  [30;  42;  115],  but  a  number  of 
approximate  solutions  have  been  developed  [52;  63],  and  this  area  eontinues  being  investigated  in 
roboties  due  to  its  parallels  with  the  problem  of  eontroller  design. 

Fortunately,  the  work  in  eonstrueting  the  state  lattiee  ean  be  performed  off-line,  without  affeet- 
ing  planner  runtime.  Onee  it  is  eonstrueted  and  represented  eompaetly  with  a  eontrol  set,  the  state 
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lattice  is  readily  used  by  the  seareh  algorithm  that  omits  eonsideration  of  differential  eonstraints. 

3.3  Properties 

In  this  seetion  we  review  the  properties  of  the  proposed  type  of  motion  primitives,  ineluding  their 
advantages  and  limitations.  This  presentation  direetly  addresses  the  ehallenges  of  the  problem  that 
have  been  identified  at  the  outset  (Seetion  1.2.2). 

3.3.1  Feasibility 

As  suggested  earlier,  the  feasibility  of  the  motion  plans  eomputed  using  the  state  lattiee  is  guaran¬ 
teed  by  the  design  of  the  motion  primitives  (developed  to  be  feasible  motions)  and  by  the  appro¬ 
priate  dimensionality  of  the  roadmap  vertiees  that  guarantees  state  eontinuity  at  the  eonneetions  of 
the  motion  primitives. 

However,  in  most  realistie  seenarios  in  roboties,  the  planned  trajeetory  is  rarely  exeeuted  verba¬ 
tim.  There  are  frequent  perturbations  due  to  unmodelled  effeets  and  uneertainty  both  in  the  models 
of  the  environment  and  the  vehiele  itself.  As  deseribed  in  1.2.1,  a  standard  approaeh  to  resolv¬ 
ing  this  diffieulty  is  by  introdueing  a  separate  trajeetory  following  eontroller  module.  It  generates 
eorreetive  input  based  on  error  feedbaek  to  ensure  that  the  planned  trajeetory  is  followed  elosely. 
Ineluding  a  trajeetory  following  eontroller  in  the  design  does  not  offset  the  value  of  planning  fea¬ 
sible  motions,  sinee  infeasible  ones  are  impossible  to  follow,  as  suggested  in  Example  1.2.2. 

Another  matter  of  eoneem  with  respeet  to  feasibility  is  that  only  approximate  BVP  solvers  are 
available  for  most  non-trivial  systems.  As  a  eonsequenee,  eomputed  trajeetories  may  not  eonneet 
the  lattiee  states  exaetly,  and  small  gaps  may  exist  at  the  eonneetions.  Even  though  these  gaps  may 
be  elosed  linearly,  it  is  nevertheless  required  that  the  overall  trajeetory  be  Lipsehitz  eontinuous 
with  a  eonstant  that  is  reasonably  small  in  order  to  make  sure  that  the  trajeetory  would  lend  itself 
to  following  with  reasonable  aeeuraey  [117].  This  evaluation  may  require  extensive  eomputation, 
but  it  takes  plaee  off-line  and  does  not  affeet  the  eomplexity  of  on-line  planning. 

Signifieant  disturbanees  in  the  environment,  sueh  as  dynamies  effeets  due  to  moving  on  slopes 
or  in  wind,  may  be  overwhelming  for  a  eontroller,  but  they  may  be  aeeounted  for  in  planning  as 
additional  state  variables.  If  these  effeets  ean  be  modeled  in  a  diseretized  fashion,  they  would 
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naturally  fit  in  the  state  lattice  framework.  For  example,  different  control  sets  may  be  developed 
for  various  discrete  values  of  these  effects,  without  any  significant  increase  in  computational  com¬ 
plexity  (notice  that  this  design  change  would  not  increase  the  number  of  motion  alternatives  that 
are  to  be  evaluated  during  planning). 

3.3.2  Optimality 

As  with  most  discretization  techniques,  a  certain  loss  in  the  attainable  minimization  of  trajectory 
cost,  with  respect  to  continuum,  results  from  the  constraint  that  the  motions  are  arranged  in  a 
particular  manner.  For  example,  minimizing  the  length  of  primitives  may  be  more  challenging  if 
a  constraint  on  endpoint  arrangements  is  placed.  Even  if  an  optimal  search  technique  is  used,  e.g. 
A*  [104],  the  computed  motion  is  only  optimal  with  respect  to  the  chosen  motion  discretization. 
However,  as  suggested  in  Section  1.2.1,  this  trade-off  is  frequently  made  in  this  domain,  since 
exact  solutions  are  unknown  for  most  of  the  systems  of  practical  interest. 

Moreover,  the  freedom  of  choosing  any  desirable  state  sampling  is  an  important  benefit  and 
may  allow  fitting  the  search  space  to  the  known  structure  of  the  environment.  This  strength  of  the 
approach  was  utilized  recently  to  fit  search  spaces  to  such  settings  as  parking  lots  [90],  roads  [118], 
mines  [34]  and  indoor  environments  [119]. 

3.3.3  Runtime 

The  runtime  advantage  of  the  proposed  method  is  an  especially  salient  one.  It  is  enabled  in  two 
important  ways: 

•  Off-line  pre-computation  of  most  of  the  components  of  the  motion  planner,  including  the 
good  state  and  control  sampling,  trajectory  cost  evaluation,  and  even  the  search  heuristic; 

•  Compatibility  with  some  of  the  most  efficient  planning  methods,  including  incremental  and 
anytime  search,  multi-directional  search  etc. 

The  pre-computation  property  is  afforded  by  the  lattice  regularity  property  that  allows  design¬ 
ing  primitives  to  be  position-invariant.  Experience  with  fielded  applications  demonstrated  that 
position-invariance  assumptions  are  often  satisfied  in  practice,  as  long  as  a  trajectory  following 
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controller  absorbs  external  perturbations  [65;  66;  81;  114].  Once  computed,  position-invariant 
primitives  can  be  utilized  anywhere  in  the  search  space,  thereby  moving  integration  of  the  controls 
to  the  planner  design  phase.  This  is  more  efficient  than  affine  invariance  [43;  46],  since  transfor¬ 
mation  of  primitives  during  search  is  limited  to  translation. 

Compatibility  with  a  wider  range  of  search  algorithms  than  is  typically  attainable  in  planning 
with  differential  constraints  is  afforded  by  the  structured  reachability  tree  pruning  rules.  Motion 
primitives  have  been  previously  designed  by  sampling  the  control  space  directly  [11;  60].  Such  ap¬ 
proaches  typically  guarantee  feasibility  by  elaborating  the  primitives  G  £  by  choosing  a  control 
Us  &  U  and  integrating  (1.2.1)  for  a  certain  At.  The  successor  vertex  is  established  at  the  end¬ 
point  of  es-  Since,  without  special  considerations  [106],  such  motions  and  their  endpoints  will  be 
dense  in  X,  these  planners  attempt  to  prune  the  edges  that  are  very  similar,  redundant  or  otherwise 
do  not  contribute  to  efficient  exploration  of  X.  For  example,  if  the  endpoint  of  a  certain  edge  es  is 
a  vertex  v^,  then  a  second  edge  terminating  in  is  discouraged  if  distance  p(v5,  v()  is  small.  To 
this  end,  these  approaches  establish  a  discretization  of  X  into  cells,  as  shown  in  Figure  3.3.1.  If  a 
cell  contains  v^,  then  is  ignored  if  would  occupy  the  same  cell.  The  proposed  lattice  primitives 
may  be  used  identically,  except  that  the  need  to  detect  similar  edge  end-points  is  eliminated,  since 
all  motions  are  designed  to  arrive  at  specific  state  values,  e.g.  Vc  in  Figure  3.3.1.  A  similar  pruning 
rule  is  in  effect  in  this  setting,  except  that  it  is  developed  off-line  during  search  space  design.  As 
Chapter  4  further  discusses,  the  application  of  a  variety  of  attractive  search  techniques,  such  as 
incremental  search  [71],  incremental  sampling  [88]  and  bi-directional  search  [75]  is  not  possible 
without  enforcing  the  graph  structure  as  proposed  here. 

One  drawback  that  may  be  experienced  with  the  proposed  primitives  is  the  potentially  signifi¬ 
cant  computation  that  may  be  required  to  design  this  type  of  primitives  (perhaps  running  the  BVP 
solver  repeatedly).  However,  this  computation  is  off-line  and  does  not  affect  the  runtime  of  the 
planner. 

3.3.4  Completeness 

By  providing  the  freedom  to  choose  an  arbitrary  sampling  of  state  space  for  primitive  endpoints, 
high  quality  state  sampling  policies  (low  discrepancy  and  dispersion)  may  be  utilized,  leading 
to  efficient  exploration  of  state  space  during  search.  State  space  is  typically  simpler  to  sample 
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effectively  than  control  space.  However,  the  resolution-completeness  property  is  not  guaranteed 
because  continuously  increasing  state  sampling  resolution  does  not,  in  the  limit,  necessarily  also 
take  control  sampling  resolution  towards  its  limit  [135].  It  is  important,  however,  that  the  sampling 
in  control  space  is  directly  induced  by  the  state  lattice,  as  motions  that  lit  the  lattice  are  found 
a  posteriori.  Thus,  sampling  effectiveness  of  the  state  lattice  is  inherited  in  control  sampling 
to  a  greater  degree  than  other  approaches  that  only  use  state  sampling  as  a  pruning  rule  [11]. 
As  Figure  3.2.2  illustrates,  the  resulting  primitives  feature  good  path  diversity,  suggesting  good 
sampling  quality  [31].  As  described  in  Chapter  8,  this  was  also  observed  during  experimentation 
in  simulation  and  with  real  robots. 


Figure  3.3.1:  State  cell  predecessors.  Three  control-sampling  primitives,  edges  em¬ 

anate  from  their  corresponding  predecessor  vertices  {vp,Vp,v"}  and  arrive  at  successor  vertices 
{vj,v',v"}.  The  square  cell  represents  a  region  in  state  space  used  for  reachability  tree  pruning 
[11].  As  all  three  successors  land  in  the  same  cell,  they  are  considered  to  be  identical.  Without 
special  considerations  to  designing  primitives,  successor  states  will  be  dense.  State  lattice  primi¬ 
tives,  however,  may  be  steered  to  converge  to  the  same  state  Vc,  leading  to  a  structure  that  can  be 
exploited  for  efficient  planning. 


CHAPTER 


SEARCH  TECHNIQUES 


This  chapter  is  devoted  to  a  discussion  of  the  details  of  applying  standard  search  techniques  to  state 
lattices.  As  was  mentioned  earlier,  the  essential  principle  of  the  present  approach  is  that,  with  the 
definition  of  the  search  space  we  have  constructed,  motion  planning  is  reduced  to  unconstrained 
search  in  this  space.  The  goal  of  this  chapter  is  to  review  several  established  search  algorithms  and 
describe  how  they  can  be  applied  in  the  context  of  state  lattice  search  spaces. 

4.1  Deterministic  Search 

Since  the  control  set  is  an  implicit  representation  of  the  search  space,  the  roadmap  is  incremen¬ 
tally  elaborated  during  search,  so  that  only  the  states  that  are  explored  by  the  search  are  stored  in 
memory.  Any  systematic  graph  search  algorithm  is  appropriate  for  finding  a  path  in  the  lattice.  It 
is  typically  desired  that  the  planner  return  optimal  paths  with  respect  to  the  desired  cost  criterion 
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and  it  be  efficient.  This  section  discusses  A*  [51]  and  D*  [71;  130]  search  algorithms  applied  to 
state  lattices,  as  they  are  some  of  the  more  popular  among  the  deterministic  search  algorithms. 

The  strengths  of  deterministic,  exhaustive  search  include  attractive  guarantees,  such  as  opti¬ 
mality  (under  certain  conditions,  such  as  heuristic  admissibility)  and  resolution-completeness,  if 
afforded  by  the  search  space.  One  drawback,  however,  is  the  so-called  “curse  of  dimensionality”, 
the  exponential  growth  of  complexity  with  dimension  of  the  search  space.  Nevertheless,  this  search 
technique  remains  attractive  for  systems  that  can  be  modeled  well  in  several  dimensions,  including 
car- like  [90],  tracked  [34],  flying  [46]  and  other  systems  of  practical  interest. 

4.1.1  Trajectory  Swaths 

It  is  widely  accepted  that  one  of  the  most  computationally  intensive  procedures  in  planning  is 
collision  detection  and  estimating  the  motion  cost.  In  order  to  compute  this  cost  accurately,  it 
is  necessary  to  simulate  the  behavior  of  the  vehicle,  subject  to  the  corresponding  control  in  the 
environment.  In  mobile  robotics  applications,  the  cost  of  robot  motion  is  often  represented  via  a 
workspace-based  cost  map,  as  suggested  in  Section  1.2.1.  Then,  the  cost  estimation  is  achieved  by 
convolving  the  vehicle  frame  along  the  workspace  projection  (path)  of  the  edge  in  question  (Figure 
4.1.1).  Since  the  motion  alternatives  can  be  pre-computed,  their  paths  (workspace  projections)  can 
be  cached  as  well,  perhaps  in  the  form  of  trajectory  swaths,  the  set  of  cost  map  cells  C  that 
are  occupied  by  the  robot  footprint  during  motion  (gray  cells  in  Figure  4.1.1).  Hence,  instead  of  the 
costly  vehicle  simulation,  perhaps  using  Runge-Kutta  or  similar  methods,  edge  cost  computation 
can  be  reduced  to  accumulating  the  cost  over  a  pre-computed  set  of  map  cells,  resulting  in  potential 
orders  of  magnitude  speed-up  in  overall  planning. 

4.1.2  State  Lattice  Search  Heuristics 

Well-informed  search  heuristics  have  the  potential  to  increase  the  efficiency  of  the  search  substan¬ 
tially  [108].  Developing  good  heuristics  for  planning  with  differential  constraints  is  a  challenging 
problem,  and  various  approaches  are  being  developed  [34;  41]. 

Among  the  simplest  options  for  a  heuristic  estimate  in  the  given  context  is  the  Euclidean  dis¬ 
tance  metric.  Weighted  Euclidean  distance,  where  dimensions  are  scaled  differently,  and  Maha- 
lanobis  distance  can  also  be  applied.  Such  metrics  are  computationally  efficient  and  can  be  defined 
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Figure  4.1.1:  Trajectory  swath.  One  of  the  motion  primitives  from  a  state  lattice  developed  for 
a  tractor-trailer  robot  is  shown.  It  is  a  maneuver  that  re-orients  the  robot  by  90°  in  heading.  The 
trajectory  swath  was  computed  using  Runge-Kutta  simulation  and  cached  off-line.  During  on-line 
planning,  the  evaluation  of  the  cost  of  this  maneuver  with  respect  to  a  cost  map  is  reduced  to  a 
summation  operation  over  a  memory  array,  resulting  in  approximately  lOOx  speed-up. 

to  satisfy  the  admissibility  requirement  [108].  However,  in  many  cases,  such  approaches  vastly 
underestimate  the  true  path  length  in  the  lattice,  resulting  in  inefficient  search. 

Better  informed  heuristics  can  be  designed  by  using  the  vehicle  controller.  Since  this  module 
typically  is  endowed  with  the  system  model,  it  has  the  potential  of  estimating  the  heuristic  cost  of 
a  motion  more  accurately.  However,  as  Example  4.1.1  illustrates,  even  though  a  heuristic  based  on 
Reeds-Shepp  distance  [115]  is  a  much  better  informed  one  for  a  car- like  robot  than  the  Euclidean 
distance,  it  still  suffers  from  significant  inaccuracy. 

Similar  to  trajectory  swaths  in  Section  4.1.1,  state  lattice  structure  enables  a  straight-forward 
pre-computation  of  the  free-space  costs  of  motions,  stored  in  a  look-up  table,  leading  to  a  very 
well-informed  heuristic  in  terms  of  actual  cost  of  feasible  motions  [69;  90;  109].  Such  a  Heuristic 
Eook-Up  Table  (HEUT)  can  be  setup  as  a  multi-dimensional  array,  indexed  by  Xf  (assuming  x/  is 
at  the  workspace  origin).  Each  element  of  this  array  is  a  scalar  that  represents  the  cost  of  steering 
the  system  from  the  origin  to  xp,  in  free  space.  Given  a  state  space  ball  of  certain  radius  around 
the  origin,  a  set  of  roadmap  vertices  in  the  ball  is  given  by  lattice  structure,  and  the  heuristic  value 
is  pre-computed  for  each  of  the  vertices.  By  the  translation  symmetry,  heuristic  evaluation  during 
on-line  planning  is  reduced  to  a  simple  table  dereference.  This  approach  enables  well-informed 
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and  fast  heuristic  evaluation  in  the  state  lattice  structure. 

An  inherent  limitation  of  pre-computing  the  heuristic  dataset  is  the  memory  requirement  for 
storage.  Even  though  the  cost  and  amount  of  computer  memory  is  continuously  increasing,  it  is 
interesting  to  seek  methods  to  alleviate  this  requirement.  One  approach  is  to  utilize  an  approxi¬ 
mate  HLUT  that  has  more  sparse  sampling  than  the  planner  search  space.  The  samples  that  are 
omitted  from  the  HLUT  may  be  interpolated.  In  this  manner,  it  is  possible  to  alleviate  the  storage 
requirement  significantly,  while  still  preserving  much  of  the  efficiency  of  this  heuristic  method. 

Example  4.1.1  (HLUT  Heuristic  for  a  Car-like  Robot).  This  example  illustrates  the  HLUT  Heuris¬ 
tic  and  contrasts  it  with  other  popular  types  of  heuristics  in  this  domain.  For  simplicity  of  expo¬ 
sition,  the  car-like  system  in  Example  1.2.1  is  reused  here.  State  space  is  assumed  to  be  four¬ 
dimensional,  including  2D  position,  heading  and  steering  angle.  Path  length  is  chosen  as  cost 
of  motion.  Recall  that  here  we  look  at  free-space  heuristics  (no  assumptions  about  the  environ¬ 
ment  are  being  made),  so  obstacles  were  not  considered  in  the  comparison.  An  illustration  of  the 
resulting  HLUT  is  given  in  Figure  4.1.2. 

Figure  4.1.3  compares  HLUT  to  two  other  heuristic  candidates.  Euclidean  and  Reeds-Shepp 
distance.  It  shows  the  ratios  of  the  distance  given  by  these  two  metrics  to  path  length  pre-computed 
in  the  HLUT. 

Euclidean  distance  is  very  fast  to  evaluate,  but  unfortunately  results  in  a  poor  estimate  of  the 
car-like  path  length.  Reeds-Shepp  distance  results  in  significantly  better  heuristic  estimates,  but 
it  still  notably  lower  than  the  HLUT  values  (Figure  4.1.3).  This  metric  assumes  that  the  vehi¬ 
cle  utilizes  only  three  controls:  turns  left  and  right  at  the  maximum  value  of  steering  angle  and 
straight  line  motion.  This  leads  to  paths  that  are  discontinuous  in  curvature  (i.e.  infeasible  to 
execute  without  stopping).  Even  though  continuous  motion  results  in  longer  path  length,  it  is  nev¬ 
ertheless  typically  preferred  over  discontinuous  alternatives  in  practice.  For  the  car-like  system, 
utilizing  a  pre-computed  HLUT  results  in  over  lOOx  speed-up  in  planning  runtime,  as  Figure  4.1.4 
demonstrates. 

To  conclude  this  example,  the  performance  of  two  variants  of  approximate  HLUT  are  demon¬ 
strated.  One  removes  the  steering  angle  dimension  of  the  initial  state,  xj,  for  each  of  the  queries 
being  considered.  Instead  of  storing  that  value,  it  sets  all  values  of  xj  ’s  steering  angle  to  zero. 
As  expected,  this  approximation  leads  to  a  certain  runtime  increase  during  planning,  as  Figure 
4.1. 5a)  shows.  A  similar  experiment  was  reproduced  by  removing  the  final  steering  angle  dimen- 
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Figure  4.1.2:  Car-like  HLUT.  A  visual  representation  of  an  HLUT  for  a  car-like  robot  from 
Example  1.2.1.  Figure  shows  a  3D  view  of  the  dataset,  where  the  Z-axis  represents  the  cost  (path 
length)  of  moving  the  car  from  the  origin  position  (the  center  of  the  plane  defined  by  the  X  and  Y 
axes)  to  a  different  position  with  the  same  orientation.  Note  that  moving  along  the  initial  orientation 
entails  low  cost,  but  moving  perpendicular  to  it  (“sideways”)  incurs  higher  cost  due  to  a  maneuver 
that  is  required  under  nonholonomic  constraints. 


sion  in  Figure  4.1.5b).  The  results  suggest  that  removing  the  final  steering  angle  dimension  results 
in  a  smaller  increase  in  runtime,  and  is  therefore  preferred.  □ 


4.2  Incremental  Search 

In  many  applications  featuring  physical  robots,  it  is  beneficial  to  perform  incremental  search: 
once  a  plan  is  computed,  it  is  incrementally  updated  (ideally,  while  reusing  previous  computa¬ 
tion)  should  new  information  about  the  environment  invalidate  it  [71;  130].  This  capacity  enables 
the  planner  to  react  quickly  to  the  changes  of  the  world  model,  including  those  due  to  uncertainty 
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Figure  4.1.3:  Heuristic  comparison.  Euclidean  and  Reeds-Shepp  distance  metrics  are  compared 
with  the  HLUT,  in  the  context  of  a  car-like  system.  The  units  of  each  of  the  three  metrics  were 
maintained  to  be  the  same.  Reeds-Shepp  and  Euclidean  distances  are  fairly  significant  under¬ 
estimates  of  the  true  path  of  the  car-like  robot.  This  observation  motivates  an  inquiry  into  better- 
informed  heuristics  for  this  type  of  motion  planning. 


and  noise  of  the  perception,  localization  and  other  systems  -  a  frequent  and  inevitable  phenomenon 
in  robotics.  This  type  of  search  is  a  typical  component  in  many  fielded  robotics  systems,  since  plan 
repair  can  be  vastly  more  efficient  than  replanning  from  scratch  [64;  89;  129]. 
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Figure  4.1.4:  HLUT  vs.  Euclidean  Comparison.  This  figure  plots  the  (seeonds)  runtime  ratios, 
of  exeeuting  a  set  of  motion  planning  queries  with  a  ear-like  robot  model  with  two  A*  planners, 
where  the  only  differenee  was  the  ehoiee  of  the  heuristie:  HLUT  and  Euelidean  distanee.  The  plot 
demonstrates  that  utilizing  an  HLUT  leads  to  over  lOOx  speed-up  in  planning  runtime. 


4.2.1  Managing  Predecessors 

As  discussed  in  Section  3.3.3,  motion  primitives  that  are  designed  by  sampling  the  control  space 
without  regard  for  state  space  structure  typically  lead  to  tree-shaped  search  spaces  [11;  60;  87].  In 
contrast,  the  most  efficient  formulation  of  incremental  search  requires  cyclic  graph  topology.  The 
operation  of  such  search  requires  an  ability  to  enumerate  and  keep  track  the  edges  that  arrive  at 
a  particular  state,  e.g.  the  edges  {es,e'^,e”}  leading  to  a  cell  containing  in  Eigure  3.3.1.  A  key 
component  of  incremental  search  algorithms  is  the  rfi^-value,  the  one-step  look-ahead  cost  value, 
that  is  defined  as  in  [71]: 
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rhs  (Vi) 


0  if  —  Xinit 

^KpEPredM  isivp)  +  c(vp,  V,) )  Otherwise 


(4.2.1) 


where  g{vp)  denotes  the  cost  of  the  vertex  Vp,  Predivs)  is  the  set  of  the  predecessors  of  v^,  e.g. 
{vp,Vp,v"}  in  Figure  3.3.1.  One  of  the  ways  of  reusing  previous  computation  is  the  capacity  to 
pick  a  different  predecessor  of  a  state  in  the  event  that  the  current  predecessor  edge  increases 
its  cost.  However,  as  illustrated  in  Figure  4.2.1,  the  predecessor  edges  cannot  be  swapped  in 
general  because  the  distance  between  their  endpoints  p  (v^,  v' )  ^  0  (by  their  density).  The  Lipschitz 
continuity  argument  may  be  invoked  to  ignore  this  gap,  but  it  is  not  clear  that  a  bound  on  the 
Lipschitz  constant  may  be  guaranteed  a  priori.  This  issue  is  resolved  with  motion  primitives 
that  conform  to  graph  structure  in  X\  this  distance  is  zero,  since  all  predecessors  converge  to  the 
identical  Vc-  Note  that  this  process  of  managing  predecessors  is  similar  to  resolution  equivalence 
in  traditional  applications  of  incremental  search  in  grids,  as  analyzed  by  Tompkins  [133]. 


4.2.2  Processing  Edge  Cost  Updates 

Moreover,  in  grids,  it  is  typically  easy  to  determine  the  set  of  cells  (and,  consequently,  edges)  that 
are  affected  when  a  region  in  the  workspace  changes  cost.  With  motion  primitives  of  arbitrary 
length  and  form,  this  computation  is  more  involved,  since  the  edges  may  span  several  cells.  How¬ 
ever,  by  structure  regularity,  this  computation  can  be  done  a  priori.  Once  we  compute  the  swaths 
of  the  motion  primitives,  we  collect  and  store  a  list  of  edges  that  pass  through  the  origin  cell.  By 
translation  symmetry,  this  list  may  be  reused  anywhere  else  in  the  search  space. 

More  formally,  for  every  change  in  the  cost,  c{xi,Xj),  of  the  directed  edge  from  the  vertex  Xi  to 
Xj,  a  replanning  algorithm  requires  recomputing  the  priority  of  xj  and  potentially  inserting  it  into 
the  priority  queue.  Assuming  a  map  cell  niij  G  changes  cost,  the  planner  needs  to  know  the 
set  of  vertices  Vc  that  potentially  need  to  be  re-inserted  into  the  priority  queue  with  new  priority. 
Thus,  the  planner  requires  a  mapping  7  :  — )■  Vc.  This  mapping  is  simple  in  the  case  of  classical 

applications  of  D*  in  2D  grids,  but  it  is  nontrivial  in  the  case  of  the  state  lattice. 

To  develop  this  mapping,  we  need  to  compute  the  swath  of  a  motion,  a  set  of  cost  map  cells 
Cs  C  that  are  covered  by  the  robot  as  it  executes  the  motion.  The  cost  of  an  edge  that  represents 
this  motion  is  directly  dependent  on  the  costs  of  map  cells  in  Q.  Once  we  pre-compute  the  control 
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set,  it  is  also  possible  to  pre-eompute  the  swaths  of  the  edges  in  it.  Once  we  have  a  mapping  from 
a  set  of  edges  in  the  control  set  to  the  map  cells  in  their  swaths,  that  mapping  is  then  inverted  to 
obtain  a  set  of  edges  that  are  affected  for  a  map  cell.  By  translation  symmetry,  this  set  of  edges  is 
the  same  for  every  map  cell.  It  can  be  precomputed  for  the  same  reasons  as  the  control  set  itself 
and  can  be  used  readily  to  incorporate  cost  map  changes  in  re-planning. 

Example  4.2.1  (Nonholonomic  D*).  This  example  illustrates  the  use  of  the  proposed  state  lattice 
search  space  to  enable,  for  the  first  time,  the  application  of  D*  search  algorithm  [70;  130]  to 
motion  planning  with  nonholonomic  constraints.  The  car-like  robot  model  from  Example  1.2.1  is 
once  again  reused  for  this  example.  Figure  4.2.2a)  shows  the  initial  plan  that  involved  a  large 
number  of  vertex  expansions  (gray  cells).  A  point  obstacle  was  introduced  that  would  cause  the 
robot  (red  6-wheeled  vehicle  frame)  to  collide,  if  it  were  to  execute  this  path.  Figure  4.2.2b)  shows 
the  motion  plan  after  replanning.  Runtime  and  plan  cost  values  are  given  in  Table  4.1.  Note  the 
significant  reduction  in  planning  time  due  to  reusing  previous  computation  and  a  slight  increase  in 
the  cost  of  the  motion  due  to  the  generated  obstacle  avoidance  maneuver.  Figure  4.2.3  illustrates 
the  predecessor  invalidation  set  that  was  pre-computed  for  this  system.  □ 


Plan  Type 

Iterations 

Runtime,  sec. 

Motion  Cost 

Initial  plan 

1 1040 

6.0 

66.021 

Replan 

512 

0.3 

66.053 

Table  4.1:  Nonholonomic  D*.  Runtime  and  cost  data  for  the  test  in  Figure  4.2.2.  Note  the  signif¬ 
icant  reduction  in  planning  time  due  to  reusing  previous  computation  and  a  slight  increase  in  the 
cost  of  the  motion  due  to  the  generated  obstacle  avoidance  maneuver. 


4.3  Incremental  Sampling 

The  advent  of  randomized  planning  in  recent  years  has  spurred  inquiry  into  effective  sampling 
methods  that  avoid  the  “curse  of  dimensionality”  while  offering  better  solution  quality  and  com¬ 
pleteness  guarantees  than  standard  randomized  approaches.  Deterministic  incremental  sampling 
techniques  have  been  proposed  as  viable  alternatives  [21;  73;  88].  One  of  them  is  the  Halton  points, 
a  J-dimensional  generalization  of  the  van  der  Corput  sequences  of  d  bases,  one  for  each  coordinate 
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[50].  A  basic  version  of  such  incremental  sampling  in  square  grids  can  be  thought  of  as  increasing 
discretization  resolution  by  a  factor  of  2^',  i  G  N. 

The  planners  that  do  not  enforce  structure  in  edge  connectivity  would  be  required  to  regenerate 
the  plan  from  scratch  every  time  the  sampling  resolution  is  incremented.  However,  lattice  prim¬ 
itives  enable  the  reuse  of  previous  computation  via  the  same  mechanism  that  allows  incremental 
search.  Due  to  regular  structure,  the  connections  between  primitives  belonging  to  different  reso¬ 
lution  levels  become  trivial,  thereby  allowing  the  results  of  planning  at  different  resolution  to  be 
reused.  One  application  of  this  approach  is  anytime  planning,  where  the  quality  of  the  computed 
plan  is  improved  with  more  computation. 

4.4  Multi-Resolution  Search 

By  virtue  of  the  state  lattice’s  general  representation  as  a  directed  graph,  it  naturally  can  be 
extended  with  multi-resolution  enhancements.  Significant  planning  runtime  improvement  was 
achieved  in  the  literature  via  a  judicious  use  of  the  quality  of  representation  of  the  planning  prob¬ 
lem,  e.g.  [18;  36;  105;  134]  among  others.  In  field  robotics,  it  is  frequently  beneficial  to  utilize 
a  high  fidelity  of  representation  in  the  immediate  vicinity  of  the  robot  (perhaps  within  its  sensor 
range),  and  reduce  the  fidelity  in  the  areas  that  are  either  less  known  or  less  relevant  for  the  plan¬ 
ning  problem.  Lower  fidelity  of  representation  is  designed  to  increase  search  speed,  while  higher 
fidelity  provides  better  quality  solutions.  Since  traditionally  grids  have  been  utilized  in  replanning, 
the  notion  of  varying  the  quality  of  problem  representation  has  been  identified  with  varying  the 
resolution  of  the  grid.  However,  this  thesis  relies  on  the  discretization  of  both  the  state  and  mo¬ 
tions.  Since  the  term  “resolution”  is  typically  used  in  2D  scenarios,  we  have  termed  managing  the 
fidelity  of  state  lattice  representation  as  graduated  fidelity . 

In  designing  the  connectivity  of  regions  of  different  fidelities,  care  must  be  taken  to  ensure  that 
all  fidelities  consist  of  motions  that  are  feasible  w.r.t.  the  robot’s  mobility  model.  If  this  rule  is 
violated,  mission  failures  become  possible  due  to  limit  cycling  at  the  border  of  fidelity  regions  due 
to  obstacles,  as  suggested  in  Example  1.2.2.  In  order  to  avoid  such  difficulties,  it  is  sufficient  to 
ascertain  that  all  levels  of  fidelity  include  feasible  motions.  For  example,  the  connectivity  of  low 
fidelity  regions  could  be  a  strict  subset  of  that  of  the  high  fidelity  regions. 

To  implement  graduated  fidelity  planning  and  efficient  replanning  in  a  manner  similar  to  the 
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framed-quadtree  multi-resolution  planning  [134],  the  presented  design  requires  almost  no  modifi¬ 
cation.  Once  the  state  lattice  graph  is  separated  into  regions  of  different  fidelity  as  desired,  each 
region  uses  its  own  control  set  to  achieve  the  chosen  fidelity  (resolution  of  vertices  and  connectiv¬ 
ity  of  edges  between  them).  Each  control  set  provides  the  successors  of  a  vertex  being  expanded 
during  search.  Care  must  be  taken  to  design  the  control  sets  such  that  they  would  adequately  span 
the  boundaries  between  the  regions.  Note  that  control  set  design  is  the  sole  procedure  needed  to 
enable  graduated  fidelity.  The  search  algorithm  requires  no  changes  and  will  achieve  the  desired 
effects  automatically. 

Moreover,  thanks  to  graph  representation  of  the  state  lattice,  it  is  also  fairly  straight-forward 
to  extend  the  concept  of  graduated  fidelity  by  allowing  the  different  regions  of  fidelity  to  move  or 
change  shape  between  replans.  This  becomes  useful  to  enable  the  high  fidelity  region  to  remain 
centered  around  the  moving  vehicle.  If  the  search  space  remains  fixed  to  the  world  frame,  a  subset 
of  the  search  space  changes  connectivity  as  it  moves  along  with  the  vehicle.  Such  flexibility  results 
in  a  dynamic  search  space,  which  complements  dynamic  replanning  algorithms  to  allow  greatly 
increased  planning  efficiency. 

As  shown  in  [1 1 1],  enabling  such  flexibility  in  the  search  space  once  again  requires  hardly  any 
change  to  the  replanning  algorithm.  The  change  of  connectivity  between  replans  is  presented  to 
the  algorithm  as  a  change  in  cost  of  the  affected  graph  vertices,  similar  to  the  change  in  cost  due  to 
perception  updates.  The  two  kinds  of  cost  changes  appear  identical  to  the  replanning  algorithms, 
and  they  are  equally  efficient  in  handling  both.  Thus,  the  graduated  fidelity  extension  of  state  lattice 
planning  is  conceptually  simple  and  easy  to  implement,  while  offering  unprecedented  planning 
efficiency  under  differential  constraints. 

In  this  section,  we  describe  an  approach  to  reducing  the  computational  complexity  of  planning 
with  state  lattices  at  the  cost  of  selectively  reducing  the  quality  of  representation.  Some  regions  of 
the  workspace,  e.g.  those  that  are  partially  or  completely  unknown,  can  be  represented  at  lower 
fidelity,  and  the  regions  that  are  best  known  or  most  relevant  for  the  problem  are  represented  at 
highest  fidelity. 

We  extend  the  above  definition  of  the  state  lattice  by  assuming  that  it  consists  of  subgraphs 
Gi,G2,...,G„.  The  arrangement  of  vertices  and  edges  in  each  subgraph  is  assumed  to  be  regular, 
but  this  arrangement  may  be  different  among  subgraphs  to  reflect  the  differences  in  the  fidelity  of 
representation.  This  composite  search  space  is  maintained  to  remain  a  directed  cyclic  graph,  so 
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that  replanning  algorithms  can  be  utilized  to  reuse  previous  computation  while  replanning. 

We  define  modifying  a  subgraph  as  arbitrarily  changing  its  position  (in  coordinates  that  do  not 
affect  its  connectivity,  namely  the  translational  ones)  and  shape  (the  extent  of  its  boundary  in  state 
space).  After  a  subgraph  is  modified,  some  of  the  vertices  near  its  boundary  are  set  to  belong  to 
a  different  subgraph.  Note  that  the  vertices  do  not  move,  they  simply  change  ownership  from  one 
subgraph  to  another.  For  example,  as  a  subgraph  changes  position,  it  “gains”  the  vertices  and  edges 
on  its  leading  interface  and  “loses”  vertices  and  edges  on  its  trailing  interface. 

After  all  subgraphs  are  modified  as  desired,  a  replanning  procedure  needs  to  be  executed  to 
repair  the  plan.  This  is  the  same  procedure  that  repairs  the  plan  due  to  other  changes  in  the  search 
space  (e.g.  a  perception  update).  To  see  why,  note  that  deterministic  replanning  algorithms  reuse 
computation  by  storing  previously  computed  costs  of  vertices  in  the  graph.  As  a  result  of  modifying 
the  subgraphs,  some  vertices  change  their  cost  due  to  their  connectivity  under  a  different  subgraph. 
It  is  entirely  transparent  to  the  replanning  algorithm  whether  they  changed  cost  due  to  new  edge 
costs  from  a  perception  update,  or  due  to  modification  of  subgraphs.  Thus,  the  only  required 
change  to  replanning  algorithms  to  enable  graduated  fidelity  is  a  process  to  make  them  aware  of 
the  vertices  that  have  new  subgraph  ownership.  This  is  performed  by  the  function  convertjvertex, 
presented  in  Algorithm  4.1. 

The  function  convert. vertex  is  executed  on  each  vertex  Vb  that  changes  subgraph  ownership. 
If  the  vertex  has  not  been  expanded  at  all  so  far,  the  function  returns.  Otherwise,  we  note  all 
vertices  that  may  contain  Vb  as  a  predecessor  -  it  is  exactly  the  set  of  successors  of  Vb  under  the 
edge  connectivity  of  its  previous  subgraph  Gi,  denoted  as  Succfvb)-  Further,  we  remove  any 
back-pointers  from  these  successor  vertices  Vj  to  Vb  by  examining  the  predecessors  of  Vj,  Pred{vs). 
Effectively,  we  undo  the  effects  of  a  previous  expansion  of  Vb-  Lastly,  if  this  change  resulted 
in  a  change  of  cost  of  any  successor  vertex,  we  insert  the  affected  vertices  and  Vb  itself  into  the 
priority  queue.  D*  variants  can  detect  this  cost  change  automatically  by  recomputing  the  r/z5-value. 
Note  that  this  procedure  is  likely  to  cause  replanning  from  the  farthest  affected  vertex  to  the  robot 
(assuming  the  search  direction  from  the  goal  to  the  robot).  Thus,  more  previous  computation  is 
reused  if  such  changes  occur  closer  to  the  robot. 

This  procedure  is  illustrated  in  Figure  4.4.1,  using  a  simplified  search  space  for  ease  of  visual¬ 
ization.  In  this  example,  the  search  space  consists  of  two  subgraphs,  Gi  (black  square  vertices)  and 
G2  (gray  circle  vertices).  Arrows  of  similar  colors  are  the  edges.  G2  is  a  small  subgraph,  consisting 
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Input:  graph  vertex  vj, 
if  Vb  was  previously  expanded  then 
foreach  G  Succi{vt)  do 
remove  from  Pred{vs)', 
update  _vertex(v5) ; 

end 

update  _vertex(vft) ; 

end 

Algorithm  4.1:  The  function  convert jvertex(vb).  It  enacts  a  change  of  subgraph  ownership  of 
the  vertex  Vb-  After  it  has  been  called  on  all  vertices  that  changed  subgraphs,  the  search  space 
becomes  ready  for  a  standard  replanning  process.  The  update wertex  function  is  assumed  to 
be  a  component  of  the  chosen  search  algorithm  that  determines  whether  a  vertex  needs  to  be 
processed  during  replanning. 


of  six  vertices,  highlighted  with  a  gray  bounding  box.  The  rest  of  the  search  space  belongs  to  Gi. 
In  this  example,  Succi{vi),  Vv,  G  Gi  is  4  nearest  neighbors,  and  Succ2{vj),  Vvj  G  G2  is  8  nearest 
neighbors.  A  real  implementation  of  graduated  fidelity  under  differential  constraints  would  utilize 
the  same  algorithm,  but  a  more  sophisticated  connectivity  of  the  subgraphs. 

The  effect  of  the  initial  plan  is  shown  in  Figure  4.4.1a).  The  search  proceeded  from  the  goal 
vertex  Vg  to  the  robot  v^.  The  vertices  with  white  centers  (“hollow”)  were  expanded  during  search, 
and  solid  vertices,  pointed  to  by  the  arrows,  remain  in  the  priority  queue.  The  resulting  motion 
plan  is  highlighted  with  a  thick  patterned  line. 

Next,  suppose  we  move  G2  to  the  right,  as  shown  in  Figure  4.4.  lb).  The  six  crossed-out  vertices 
change  subgraph  ownership  due  to  this  move,  and  we  execute  convert  jvertex  on  each  of  them.  The 
previous  expansion  of  vertices  vi  and  V2  is  undone:  their  edges  (dotted  arrows)  are  removed.  In 
Figure  4.4.1c),  moving  G2  is  completed,  and  the  affected  vertices  are  inserted  into  the  priority 
queue.  When  the  search  algorithm  begins  replanning,  these  vertices  will  be  expanded,  using  the 
edge  connectivity  of  G2,  if  they  are  relevant  for  the  problem  at  hand,  as  deemed  by  the  heuristic. 
Figure  4.4.  Id)  completes  our  example  and  shows  a  new  motion  plan  (thick  patterned  line)  due  to 
moving  G2  to  the  right. 

The  same  algorithm  would  work  with  subgraphs  of  different  dimensionalities  by  using  addi¬ 
tional  “connecting”  edges.  They  are  used  as  part  of  the  expansion  of  a  vertex  that  lies  on  the 
boundary  of  a  subgraph,  in  order  to  connect  it  with  the  other  subgraph.  A  simple  example  of  con- 
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necting  a  2D  subgraph  Gi  to  a  3D  subgraph  G2  is  shown  in  Figure  4.4.2.  Additional  edges  (gray 
arrows  in  the  figure),  denoted  by  Succuivb)  are  used  in  order  to  generate  suitable  eonneetivity 
from  a  vertex  Vb  in  subgraph  Gi  to  G2.  In  this  ease,  we  have  eonneeted  this  vertex  to  several  ver- 
tiees  in  G2  representing  all  possible  values  of  the  third  dimension.  Algorithm  4. 1  ean  be  utilized 
here  with  a  minor  modifieation.  In  addition  to  using  the  vertiees  in  Succi{vb)  in  the  foreach  loop, 
we  also  use  the  vertiees  from  Succij{vb).  This  addition  eertainly  inereases  the  branehing  faetor  of 
boundary  vertiees,  however  the  eomplexity  effeet  ean  be  insignifieant  if  only  a  small  number  of 
vertiees  require  the  extra  edges. 

Example  4.4.1  (Graduated  fidelity).  To  illustrate  graduated  fidelity  concepts,  the  car-like  system 
model  from  previous  examples  is  slightly  modified:  it  is  endowed  with  a  capacity  to  turn  in  place 
similar  to  differential  drive  vehicles.  This  is  helpful  in  that  it  enables  highlighting  the  differences 
in  search  space  connectivity  in  the  high  and  low  fidelity  regions.  The  former  here  is  chosen  to  be 
just  like  the  original  car-like  system,  and  the  latter  is  a  basic  8-connected  grid  (Figure  4.4.3). 

With  this  setup,  by  applying  unmodified  D*,  nearly  an  order  of  magnitude  runtime  improvement 
is  attained,  as  shown  in  Figure  4.4.4,  while  the  vehicle  actually  never  has  to  resort  to  point  turn 
motions,  since  high-fidelity  region  travels  with  the  vehicle.  This  results  in  an  execution  advantage 
as  well,  since  a  point  turn  presumably  would  have  required  slowing  the  vehicle  to  a  complete  stop. 

□ 


4.5  Randomized  Search 

Lattiee  primitives  may  be  utilized  in  randomized  seareh  in  a  manner  that  is  similar  to  other  types 
of  primitives  [44].  The  loeal  planner  eomponent,  as  suggested  in  [55;  61;  87]  and  similar  planners, 
may  be  designed  to  ehoose  an  element  of  a  set  of  primitives  that  is  a  good  fit  to  extend  the  tree 
or  the  graph  towards  a  random  sample.  However,  by  virtue  of  the  regular  structure  of  the  state 
samples,  lattice  primitives  would  enable  additional  capacity  to  execute  parallelized  kinodynamic 
planning,  e.g.  bi-directional  [75],  as  well  as  a  series  of  independent  searches  [91].  Figure  4.5.1 
illustrates  that,  unlike  control-sampling  primitives  that  would  likely  require  multiple  BVP  solutions 
to  connect  partial  planning  results,  the  layout  of  lattice  primitives  makes  this  connection  automatic. 

Example  4.5.1  (State  Lattice  Bi-RRT).  To  illustrate  the  application  of  randomized  search  in  a 
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state  lattice,  this  example  applies  bi-directional  RRT  search  [75]  to  the  car-like  system.  The  paths 
of  the  motions  computed  by  the  RRT  and  A*  searches  for  three  different  planning  queries  are  shown 
in  Figure  4.5.2.  Performance  statistics  on  these  tests  are  given  in  Table  4.2. 

□ 


Trial  Num. 

Bi-F 

Runtime,  sec. 

LRT 

Motion  Cost 

1 

Runtime,  sec. 

* 

Motion  Cost 

1 

4 

194 

10 

73 

2 

1 

39 

5 

30 

3 

0 

42 

1 

23 

Table  4.2:  Comparison  of  a  Bi-RRT  and  A*  in  the  state  lattice. 


4.6  On-Demand  Expansion 

Two  significant  challenges  of  applying  optimal  graph  search  techniques  include  search  space  qual¬ 
ity  and  computational  complexity.  Motion  plans  generated  by  graph  search  are  only  as  good  as  the 
quality  of  the  underlying  search  space.  Search  spaces  that  represent  many  detailed  routes  through 
cluttered  environments  generally  outperform  those  with  coarser  approximations  of  all  feasible  mo¬ 
tions.  Computational  complexity  balances  out  the  desire  to  represent  very  detailed,  expressive 
search  spaces  because  it  is  expensive  in  memory  and  processing  to  represent  numerous  nodes  and 
evaluate  all  edges.  In  this  section,  an  adaptive  state  lattice  search  space  is  developed  in  a  manner 
similar  to  the  approach  by  Ferguson  and  Stentz  [35].  It  can  be  thought  of  as  the  middle  ground 
between  a  small,  canonical,  set  of  motion  primitives  that  allows  low  planning  runtime,  but  may 
suffer  from  losses  in  solution  quality,  and  a  larger,  expressive,  set  of  primitives  that  allows  better 
quality  at  the  cost  of  runtime.  The  search  algorithm  is  endowed  with  monitoring  logic  to  detect 
cases  where  most  of  the  alternatives  in  the  canonical  control  set  are  being  invalidated  due  to  envi¬ 
ronment  constraints.  In  this  case,  the  expressive  control  set  is  used  temporarily;  it’s  greater  variety 
of  motions  may  suggest  a  route  that  was  not  evaluated  by  the  canonical  set.  For  example,  a  global 
motion  planner  with  both  variable  fidelity  and  expressiveness  can  benefit  from  a  notable  runtime 
improvement  by  searching  expressively  in  perceived  regions  and  coarsely  in  inferred,  unknown,  or 
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previously  traversed  areas. 

Figure  4.6. 1  illustrates  this  idea.  The  green  paths  represent  the  pre-computed  motion  alterna¬ 
tives  for  a  canonical  (red)  path.  The  brown  obstacle  in  the  figure  illustrates  that,  even  though  the 
canonical  path  may  become  blocked,  the  path  alternatives  may  still  remain  viable  options  to  enable 
the  vehicle  to  move  towards  its  goal  while  avoiding  the  obstacle.  The  black  square  cells  illustrate 
the  lattice  cells  that  are  different  from  the  cells  covered  by  the  canonical  path.  Maximizing  the 
number  of  these  cells  (set  difference  between  the  set  of  cells  covered  by  the  canonical  path  and  the 
set  of  cells  covered  by  a  path  alternative)  is  beneficial,  since  it  increases  the  probability  that  the 
alternative  will  remain  collision  free  even  if  the  canonical  path  is  blocked.  This  observation  is  well 
in  line  with  other  results  regarding  path  diversity  in  this  setting  [20;  33;  49]. 

By  regularity  of  the  state  lattice,  a  set  of  motion  alternatives  developed  to  fit  state  discretization 
at  the  origin  can  be  reused  anywhere  else  in  the  workspace.  The  approach  has  an  added  benefit 
of  moving  the  computation  of  substituted  variants  of  canonical  motions  from  on-line  to  the  off¬ 
line  planner  design  stage.  Based  on  earlier  experience,  this  approach  alleviates  some  computation 
burden  during  optimization-enabled  navigation,  especially  for  higher-order  parametrized  motions, 
such  as  clothoids  [52]  -  at  the  cost  of  a  more  coarse-grained  adaptation  of  paths  to  the  environment. 

Given  a  pre-computed  set  of  motion  primitives  that  adheres  to  the  underlying  state  lattice  struc¬ 
ture,  we  now  turn  to  methods  of  using  it  during  planning.  The  most  straight-forward  method  of 
using  such  a  set  is  to  augment  the  standard  vertex  expansion  procedure  to  consider  motion  alter¬ 
natives,  should  the  canonical  motion  be  blocked.  For  its  application  in  this  manner,  we  make  a 
simple  modification  to  the  set  of  path  alternatives.  We  have  noted  in  prior  work  that  shorter  motion 
primitives  appear  to  lead  to  better  completeness  properties  of  the  resulting  planner.  Therefore,  we 
sort  the  motions  in  the  alternatives  set  by  path  length.  If  a  canonical  motion  is  not  collision-free, 
we  attempt  to  substitute  it  with  alternatives  from  the  set,  in  increasing  order  of  path  length.  We  stop 
this  process  when  a  collision-free  alternative  is  found.  This  motion  is  used  en  lieu  of  the  canonical 
path.  From  this  point,  the  standard  best-first  planning  process  proceeds.  Since  the  procedure  is 
very  similar  to  the  typical  vertex  expansion,  we  refer  to  it  as  on-demand  expansion  to  highlight  the 
fact  that  additional  motions  are  evaluated  only  when  necessary,  i.e.  on-demand. 

The  capacity  to  perform  on-demand  expansion,  enabled  by  the  state  lattice  structure,  is  signifi¬ 
cant  because  it  provides  for  a  greater  diversity  of  roadmap  edges  without  any  effects  on  the  vertices 
(since  all  on-demand  motion  alternatives  connect  at  the  same  vertex).  As  shown  in  Section  8.5, 
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this  feature  leads  to  as  much  as  2x  runtime  improvement  in  challenging  planning  problems. 

This  chapter  reviewed  the  details  of  applying  standard  search  techniques  to  the  search  spaces 
based  on  state  lattice  structure.  As  was  mentioned  in  previous  chapters,  the  essential  principle  of 
the  present  approach  is  that  the  proposed  search  space  structure  reduces  motion  planning  with  dif¬ 
ferential  constraints  to  unconstrained  search,  while  enabling  several  search  paradigms  previously 
under- utilized  in  this  type  of  motion  planning. 
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Figure  4.1.5:  Approximate  HLUT.  Planning  runtime  with  two  variants  of  approximate  HLUT  are 
eompared  here  with  the  exaet  HLUT.  A  set  of  identieal  planning  queries  were  eomputed  with  eaeh 
of  the  heuristies,  and  the  runtime  (seeonds)  ratios  are  plotted  on  the  vertieal  axis.  The  1000  ex¬ 
periment  repetitions  are  distributed  across  the  horizontal  axis.  Top:  HLUT  with  the  initial  steering 
angle  dimension  removed;  average  runtime  increase  w.r.t.  full  HLUT  is  7.2x.  Bottom:  HLUT  with 
the  final  steering  angle  dimension  removed;  average  runtime  increase  w.r.t.  full  HLUT  is  4.4x. 
The  latter  case  (removing  final  steering  angle  dimension)  leads  to  a  smaller  runtime  increase  and 
is  therefore  preferred. 
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Figure  4.2.1:  Motivation  for  graph  structure.  Sampling  control  space  without  regard  for  state 
space  structure  may  not  result  in  the  cyclic  graph  structure  that  is  necessary  to  reuse  previous 
computation.  For  example,  tree-based  search  spaces  where  vertices  are  dense  inX,  would  not  allow 
reusing  previous  computation  via  predecessor  redirection.  In  case  a  predecessor  edge  becomes 
invalidated  due  to  new  environment  information  (red  stop-sign  in  the  figure),  an  attempt  to  reuse 
an  alternative  predecessor  would  result  in  a  gap  in  the  trajectory  (question  mark). 
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(a)  Initial  plan 


(b)  Replan 


Figure  4.2.2:  Nonholonomic  D*.  A  simple  2D  example  of  ineremental  planning  with  D*.  An 
initial  plan  (a)  and  an  ineremental  replan  (b)  to  “fix”  the  initial  plan  that  was  bloeked  by  a  point 
obstaele  (new  point  obstaele  in  figure  plaeed  in  eollision  with  red  vehiele  frame).  Grey  eells  are 
expanded  vertiees. 
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Figure  4.2.3:  Pre-computed  invalidation  set.  A  set  of  states  that  are  affected  by  a  map  cell  that 
changes  cost  can  be  pre-computed  and  reused.  This  set  was  generated  for  the  car-like  system  in 
Example  1.2.1. 
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Figure  4.4.1:  Dynamic  search  space.  Maintaining  the  connectivity  between  two  subgraphs  of 
different  fidelities  of  representation.  Gi  is  a  static  subgraph  (black  square  vertices),  and  G2  (gray 
circular  vertices)  moves  w.r.t.  the  former.  Arrows  are  edges.  Hollow  vertices  have  been  expanded. 
Subfigure  a)  shows  the  initial  plan  (thick  patterned  line),  as  it  originates  in  Gi  and  proceeds  into 
G2;b)  as  G2  moves  from  left  to  right,  the  six  crossed  out  vertices  change  subgraph  ownership,  and 
convert  A’ ertex  is  executed  on  each  of  them,  which  results  in  undoing  the  previous  expansions  of 
vi  and  V2;  c)  shows  the  completion  of  moving  G2:  the  vertices  vi  and  V2  now  belong  to  G2  and  are 
available  for  re-expansion,  if  necessary,  when  the  search  algorithm  performs  replanning;  lastly,  d) 
shows  the  result  of  replanning  in  the  search  space  from  c),  where  due  to  re-expansion  of  vi  under 
G2  edge  connectivity,  a  new  plan  is  found. 
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Figure  4.4.2:  Spanning  dimension  boundary.  The  2D  subgraph  Gi  (4-connected  grid)  is  con¬ 
nected  to  another  subgraph  G2  of  a  higher  dimension.  Black  arrows  are  the  standard  node  expan¬ 
sion  (4  nearest  neighbors),  and  gray  arrows  are  additional  edges  that  connect  the  two  subgraphs. 


Heading 


Figure  4.4.3:  Graduated  fidelity  example.  A  simplified  representation  of  two  state  lattices  of 
different  fidelities,  seamlessly  merged  due  to  structure:  a  three-dimensional  roadmap  that  in¬ 
cludes  position  (x,y)  variables  and  heading  is  allocated  in  the  vicinity  of  the  vehicle,  and  a  lower¬ 
dimensional  one,  with  just  position  variables,  is  further  away. 
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GF  runtime  is 
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(despite  edge  modifi 
cation  overhead) 


High  Fidelity  region; 


Figure  4.4.4:  Graduated  fidelity  runtime.  Illustration  of  the  effeets  of  graduated  fidelity. 
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Figure  4.5.1:  State  Lattice  Bi-RRT.  The  problem  of  gaps  at  the  connections  of  the  leaves  of  the 
two  trees  is  eliminated  with  lattice  primitives  due  to  the  regularity  of  endpoints  in  state  space. 
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(c)  Bi-RRT 


(d)  A* 


Figure  4.5.2:  Bi-RRT  results.  Motion  plans  computed  with  Bi-RRT  search  (left)  and  A*  (right). 
The  performance  results  are  presented  in  Table  4.2. 
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Figure  4.6.1:  On-demand  expansion.  Pre-computed  sets  of  primitives  for  on-demand  expansion 
-  a  mechanism  for  managing  the  expressiveness-runtime  trade-off.  The  red  path  represents  a  single 
path  in  the  canonical  set  while  the  green  paths  represent  the  possible  alternatives. 
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CHAPTER 


SEARCH  SPACE  QUALITY  MEASURE 


As  formulated  in  Section  1.2.2,  the  goal  of  this  thesis  is  choosing  a  motion  planner  design  that 
is  attractive  with  respect  to  the  specific  challenges  that  contribute  to  most  of  the  difficulty  of  the 
problem  of  motion  plannning  with  differential  constraints. 

Towards  that  end,  a  specific  search  space  type,  the  state  lattice,  was  introduced.  Having  moti¬ 
vated  the  proposed  type  of  motion  primitives  in  Chapters  3  and  4,  this  Chapter  begins  the  discussion 
on  the  principles  of  their  design.  Assuming  a  system  model  and  an  appropriate  controller,  the  prin¬ 
ciples  are  organized  in  three  categories:  setting  up  metrics  for  comparing  alternative  search  space 
designs  in  an  effort  to  arrive  at  the  optimal  one  with  respect  to  the  design  criteria  in  Section  1.2.2 
(this  Chapter),  developing  the  appropriate  sampling  rules  in  state  space  (Chapter  6)  and,  finally, 
designing  a  near-minimal  set  of  state  lattice  primitives  (Chapter  7)  based  on  the  chosen  structure. 
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5.1  Search  Space  Design  Formulation 

Section  1 .2. 1  poses  the  motion  planning  problem  as  optimization  in  the  space  of  motion  alternatives 
that  the  system  is  capable  of,  the  control  space  U.  This  space  is  referred  to  as  search  space  because 
it  is  the  domain  of  optimization,  often  implemented  as  search.  Due  to  the  complexity  of  this 
problem,  it  is  common  practice  to  solve  its  approximate  formulation,  where  the  search  space  is 
a  sampled  subset  of  the  control  space,  il.  In  the  context  of  this  thesis,  the  search  space  is  the 
power  set  of  the  set  of  edges  in  the  state  lattice  roadmap,  U  =  By  regular  structure  and 

position-invariance,  “E  is  the  union  of  a  repeating  control  set  “Lo  at  the  origin  and  all  its  variants 
at  the  position  offsets  of  each  of  the  roadmap  vertices.  The  cardinality  of  “E  is  thus  related  to  the 
number  of  vertices  in  the  roadmap,  and  in  general  this  set  is  countably  infinite.  The  problem  of 
designing  a  search  space  is  hereby  formulated  as  finding  a  control  set  ‘Lq  such  that  the  resulting 
search  space  il  maximizes  a  certain  utility  function. 

El*  =  argmaxfF  ( EZ)  (5.1.1) 

iidu 

where  the  utility  function  is  a  sum  of  the  three  objectives  related  to  the  specific  challenges  specified 
in  Section  1.2.2:  optimality,  runtime  and  completeness: 

J^{U)  =  %{U)  +  fr{U)  +  %{U)  (5.1.2) 

The  feasibility  criterion  in  Section  1.2.2  does  not  appear  in  the  objective  function  because  all  of 
the  U  being  considered  are  assumed  to  consist  of  exclusively  feasible  motions. 

In  the  sequel,  choosing  a  maximizer  El*  will  be  identified  with  choosing  a  corresponding  con¬ 
trol  set  £o*,  since  one  set  directly  influences  the  other.  Also,  notice  that  the  maximization  domain 
above  is  the  entire  control  space  of  the  system,  which  is  difficult  to  explore.  As  suggested  above, 
this  thesis  makes  a  simplifying  assumption  that  the  El  being  considered  belong  to  the  state  lat¬ 
tice  type  of  discretized  search  spaces.  Similar  assumptions  are  common  in  this  domain  due  to  the 
difficulty  of  the  original  problem. 
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5.2  Search  Space  Pseudo-Metric 

While  the  formulation  of  search  space  design  in  the  previous  section  is  fairly  straight-forward,  it  is 
difficult  to  apply  it  in  a  realistic  setting,  unfortunately.  For  example,  it  is  not  clear  how  to  estimate 
the  components  of  the  objective  function  (5.1.2).  Instead,  it  appears  reasonable  to  re-frame  the 
objective  function  to  be  a  relative  one  that  is  based  on  comparing  two  given  search  spaces.  It  is 
more  straight-forward  to  determine  if  one  search  space  is  more  attractive  than  the  other,  based  on 
relevant  performance  qualities.  In  fact,  such  comparisons  have  already  been  made  in  this  thesis, 
e.g.  during  the  discussion  of  the  discriminators  of  the  proposed  type  of  search  spaces  with  existing 
alternatives. 

To  this  end,  a  search  space  pseudo-metric  is  proposed  as  the  map  p  :  fz  x  fz  — )■  M.  It  is  a 
relative  objective  function  that  measures  “merit”  of  one  search  space  with  respect  to  the  other. 
It  is  specifically  designed  not  to  be  an  actual  metric  because  it  is  convenient,  in  this  setting,  to 
avoid  making  it  positive  definite.  In  fact,  signed  values  of  this  function  will  gain  special  meaning: 
if  the  value  is  positive,  then  one  of  the  arguments  of  the  function  (say,  the  first  one)  is  a  search 
space  with  a  better  score  than  the  other  one,  and  if  the  value  is  negative,  then  vice  versa.  In  this 
manner,  stochastic  optimization  techniques,  such  as  simulated  annealing  or  genetic  algorithms,  can 
be  easily  constructed  to  pursue  and  further  improve  the  more  promising  of  the  two  search  space 
candidates.  If,  for  example,  one  of  the  arguments  to  p(-)  is  the  best  candidate  seen  so  far  and  the 
pseudo-metric  value  is  positive,  then  the  other  is  a  promising  new  search  space  to  consider  further. 

5.2.1  Empirical  Evaluation 

A  natural  approach  to  evaluating  p(-)  is  to  base  it  on  direct  measurements  of  its  performance  as 
part  of  an  actual  motion  planner.  Since  the  present  inquiry  focuses  on  search  space  design,  it  is 
assumed  that  the  choice  of  the  search  algorithm  is  determined  by  the  application.  Compared  to 
other  search  spaces,  it  is  a  particularly  good  assumption  for  a  state  lattice  search  space  because  it 
is  compatible  with  a  large  variety  of  algorithms  used  in  planning. 

Thus,  given  the  two  state  lattice  search  space  candidates,  Ui  and  Ui,  being  compared,  a  search 
algorithm  of  choice  is  paired  with  one  of  them.  The  resulting  motion  planner  is  used  to  compute  a 
large  number  of  planning  queries,  e.g.  for  all  vertices  in  in  a  state  space  ball  of  radius  R  around 
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the  roadmap  origin,  similar  to  the  eomputation  of  the  HLUT,  as  deseribed  in  Seetion  4.1.2.  * 

For  eaeh  planning  query,  exeeuted  with  a  motion  planner  developed  in  the  above  manner,  we 
eolleet  two  sealars:  the  eost  of  the  motion  and  the  runtime  (in  terms  of  seeonds  or  planner  it¬ 
erations);  these  eorrespond  to  the  Optimality  and  the  Runtime  seareh  spaee  eriteria  we  seek  to 
optimize.  To  evaluate  the  Completeness  eriterion,  we  reeord  the  number  of  elements  in  for 
whieh  the  planner  was  able  to  find  eollision-free  trajeetories.  This  represents  a  measurement  of 
coverage,  an  estimate  of  the  Completeness  property  via  direet  measurement  of  the  planner’s  per- 
formanee.  Even  though  the  eost  and  runtime  measurements  in  free  spaee  are  meaningful,  the 
eoverage  measurement  is  not  (reeall  that  all  state  lattiee  vertiees  are  reaehable  by  eonstruetion) 
and  must  be  earned  out  under  environment  eonstraints.  A  Monte  Carlo  evaluation  with  a  range  of 
random  worlds  is  one  way  to  obtain  this  measurement.  In  eonelusion.  Feasibility  eriterion  is  not 
evaluated  beeause  it  is  assumed  to  be  satisfied  in  this  setting  by  eonstruetion. 


Figure  5.2.1:  Empirical  ratio  metrics.  The  figure  eompares  two  different  state  lattiee  seareh 
spaees  and  indieates  that,  on  average,  one  of  the  seareh  spaees  was  as  mueh  as  40%  more  effieient 
at  eomputing  approximately  the  same  quality  of  solutions  (eost  ratio  peaks  at  1.0). 

*Choosing  a  finite  radius  in  this  manner  is  justified  by  the  third  assumption  in  Section  1.2.3.  The  value  of  this 
radius  is  chosen  as  large  as  possible  given  the  available  computation  capacity.  Note  that  a  search  space  developed  with 
a  finite  R  still  can  be  extended  indefinitely  via  repeated  application  of  the  control  set,  so  the  finite  reachability  tree 
used  for  search  space  design  does  not  sacrifice  the  global  nature  of  the  resulting  motion  planner.  The  sacrifice  here  is 
in  terms  of  the  quality  of  the  representation  of  the  reachability,  hence  the  desire  to  choose  R  as  large  as  possible. 
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Once  the  three  quantities  above  are  computed  for  a  set  of  motion  planning  queries,  the  same 
queries  are  repeated  with  the  second  planner,  based  on  the  alternative  search  space,  ‘112.  Once  the 
process  is  completed,  two  ratios  for  each  planning  query  (Optimality  and  Runtime)  and  one  ratio 
for  the  entire  set  of  queries  (Completeness)  are  computed.  Performing  statistical  analysis  on  these 
data  reveals  the  relative  performance  of  ‘Ui  with  respect  to  ‘U2.  It  is  used  as  the  basis  of  computing 
p(-).  For  example.  Figures  5.2.1  a)  and  b)  plot  the  data  in  raw  and  histogram  form,  respectively. 
The  histogram  reveals  that,  among  the  two  different  state  lattice  search  spaces  compared,  on  aver¬ 
age,  one  of  the  search  spaces  was  as  much  as  40%  more  efficient  at  computing  approximately  the 
same  quality  of  solutions  (cost  ratio  peaks  at  1.0).  Similarly,  coverage  results  can  be  represented 
as  a  histogram  over  ‘V’r,  a  subspace  of  X  that  was  used  in  the  evaluation.  Figure  8.5.3  shows  an 
example  of  such  a  histogram:  since  the  domain  of  the  histogram  is  2D,  pseudo-color  is  utilized 
to  visualize  the  coverage  in  terms  of  the  number  of  different  (discrete)  heading  values  that  are 
reachable  at  each  (v:,y)  cell. 

Certainly,  there  are  certain  drawbacks  to  a  purely  empirical  method  of  comparing  search  spaces, 
most  notably  the  involved  computation  effort,  however  it  is  beneficial  that  the  method  directly 
measures  the  qualities  that  matter  in  applying  the  resulting  motion  planner. 

At  the  outset,  the  goal  was  to  compute  a  scalar  value  to  represent  the  comparison  of  two  search 
space  candidates.  At  this  point,  we  have  obtained  histograms  for  the  three  quantities  of  interest. 
Any  method  of  reducing  a  datapoint  distribution  in  such  histograms  to  a  scalar  may  be  used.  One 
example  is  Kullback-Leibler  divergence;  an  even  simpler  method  is  a  sum  of  histogram  bar  heights 
weighted  by  the  value  the  bar  represents.  As  Example  5.2.1  illustrates,  by  formulating  the  hori¬ 
zontal  axis,  e.g.  in  Figure  5.2.1b)  on  a  log  scale,  we  obtain  the  signed  pseudo-metric  that  was 
motivated  above:  swapping  the  arguments  to  p(-)  negates  its  value. 

5.2.2  Quality  Measure 

The  set  of  vertices  in  the  roadmap  “V  is,  in  general,  countably  infinite.  The  regular  structure  of  the 
state  lattice  makes  it  possible  to  enumerate  any  R-bounded  subset  <Z  Similarly,  the  power 
set  of  edges  in  the  roadmap,  U,  defined  in  Section  5.1,  is  similarly  countably  infinite,  and  its  R- 
bounded  subset  C  £  is  finite,  albeit  very  large.  Nevertheless,  T.r  is  an  important  construct  - 
it  is  the  most  expressive  set  of  motion  alternatives  in  the  R-ball  that  the  state  lattice  is  capable  of 
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representing. 

As  the  reader  may  recall,  an  objective  function  IF  ( U)  over  search  spaces  was  introduced  earlier 
in  this  Chapter,  and  it  played  a  key  role  in  formulating  search  space  design  as  an  optimization 
problem  (5.1.2).  It  acts  as  a  measure  in  that  it  estimates  the  quality  of  the  given  search  space.  The 
pseudo-metric  in  Section  5.2  can  be  transformed  into  this  measure  using  £  reviewed  above: 

)F(‘fl)  =  p(<fz,‘E)  (5.2.1) 

In  practical  computation,  7?-ball  variants  of  U  and  £  would  be  utilized,  resulting  in  an  approx¬ 
imate  estimate  of  this  quality  measure. 

Example  5.2.1  (Search  Space  Quality  Comparison).  In  this  example,  the  empirical  formulation  of 
the  search  space  pseudo -metric  (Section  5.2.1)  is  applied  to  the  quality  comparison  of  two  search 
spaces:  a  state  lattice  and  a  Barraquand-Latombe  search  space  [11].  As  elsewhere  in  this  thesis, 
a  car-like  system  model  is  used  for  its  simplicity  (1.2.2). 

Barraquand-Latombe  search  space,  Ubl,  includes  three  motions  emanating  from  any  state:  a 
left  and  right  turn  at  the  maximum  value  of  steering  angle  and  a  straight  motion,  for  a  certain 
duration  At.  The  duration  is  set  in  such  a  way  as  to  ensure  that  each  of  the  motions  escapes 
the  state  cell  associated  with  the  vehicle’s  current  state.  State  sampling  is  three-dimensional, 
including  2D  position  and  orientation.  Position  sampling  cell  size  was  set  to  0.2  while  the  vehicle 
minimum  turning  radius  was  set  to  1.0.  Heading  dimension  was  discretized  into  16  values.  Note 
that  concatenations  of  such  primitives  results  in  discontinuous  curvature  across  connections  -  the 
robot  would  have  to  stop  at  each  transition  point  between  the  primitives  in  order  to  re-orient  its 
wheels. 

State  lattice  search  space,  Usl,  was  designed  to  match  the  above  search  space  as  closely  as 
possible:  the  same  position  and  heading  sampling  was  used.  However,  the  available  controller 
for  constructing  state  lattice  motion  primitives  allowed  setting  steering  angle  at  trajectory  end¬ 
points  [63]  -  this  capacity  was  utilized  in  order  to  allow  continuity  of  curvature  across  nodes.  The 
steering  angle  was  set  to  0  at  the  transitions  between  motion  primitives. 

The  empirical  pseudo-metric  was  evaluated  on  the  two  search  spaces  by  computing  planning 
queries  for  every  element  of  “Pr,  using  a  large  state  space  region,  R  =  20,  in  order  to  capture  the 
maneuverability  of  the  car-like  robot  as  fully  as  possible. 
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Figure  5.2.2  shows  the  results  of  comparing  the  cost  of  computed  motions:  the  state  lattice 
search  space  results  in  about  14%  greater  path  length,  largely  due  to  the  imposed  constraint  that 
curvature  is  continuous  across  transition  points.  In  addition,  the  figure  demonstrates  the  effect  of 
representing  the  cost  ratio  data  as  histograms,  generated  against  the  horizontal  axis  in  log  scale: 
swapping  the  order  of  the  arguments  to  p  reflects  the  histogram  across  the  vertical  axis  ( compare 
Figure  5.2.2  a),  and  b).),  and  negates  the  value  of  the  pseudo-metric: 

P(%L,  Usl)  =  -P(%L,  Ubl)  (5.2.2) 


(a)  p{Ubl,  ilsL)  =  -Q-IA2 


(b)  P{UslAbl)  =Q-IA2 


Figure  5.2.2:  Barraquand-Latombe  vs.  state  lattice  cost  ratios.  The  histogram  demonstrates 
a  eomparison  of  the  eost  ratios  (Optimality  eriterion)  of  two  seareh  spaees:  Barraquand-Latombe 
[11]  and  the  state  lattiee.  Both  seareh  spaees  were  eonfigured  as  similar  as  possible,  however 
Barraquand-Latombe  does  not  allow  eontinuity  of  steering  angle  aeross  motion  primitive  transi¬ 
tions,  while  state  lattice  does,  which  results  in  a  14%  path  length  advantage  of  the  former  w.r.t. 
the  latter.  The  histogram  method,  when  expressed  on  the  horizontal  log  scale,  allows  inverting  the 
pseudo-metric,  which  results  in  reflecting  the  histogram  across  the  vertical  axis  and  negating  its 
residual  value. 

An  alternative  method  of  visualizing  search  pseudo-metric  data  is  presented  in  the  Figures 
5.2.3  and  5.2.4.  Here,  cost  ratios  are  plotted  against  runtime  ratios.  Hyperbolic  shape  of  the  plots 
is  due  to  the  majority  of  the  motion  planning  queries  with  the  two  search  spaces  having  similar 
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relative  cost  and  runtime  (i.e.,  being  close  to  the  point  (1,1)  in  the  plots). 

The  data  in  these  plots  are  from  the  experiments  that  are  very  similar  to  the  one  represented 
in  Figure  5.2.2,  which  was  repeated  at  four  different  resolutions  of  heading  discretization:  values 
Nq  =  8, 16,32,80,  and  four  different  resolutions  of  steering  angle,  values  =  1,3, 5, 7  (color- 
coded  in  the  Figures).  An  example  of  the  search  space  with  Nq  =  %  and  N-^=  \  is  shown  in  Figure 
3.2.2.  These  plots  are  helpful  in  exploring  the  resolution  space  of  state  lattice  designs,  the  space  of 
alternative  state  lattice  designs  by  varying  the  sampling  resolution  of  the  state  space  dimensions, 
in  this  case  the  resolution  of  the  heading  and  steering  angle  dimensions  for  the  two  search  spaces 
being  evaluated. 

□ 

This  chapter  began  the  discussion  on  the  principles  of  the  design  of  state  lattice  motion  primi¬ 
tives  by  setting  up  the  metrics  for  comparing  alternative  search  space  designs  in  an  effort  to  arrive 
at  the  optimal  one  with  respect  to  the  design  criteria  in  Section  1.2.2.  A  data-driven  approach  was 
proposed  for  establishing  a  fair  comparison  of  two  alternative  state  lattice  search  space  designs  in 
a  way  that  is  meaningful  with  respect  to  the  design  criteria. 
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(b)  Nb  =  16 


Figure  5.2.3:  Resolution-space  analysis.  The  resolution  space  of  heading  and  steering  angle 
dimensions,  in  the  context  of  the  search  space  design  Example  5.2.1,  is  explored  here.  The  plots 
represent  cost  vs.  computation  ratios  plotted  against  each  other. 
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(b)  Nq  -  80 


Figure  5.2.4:  Resolution-space  analysis  (cont).  The  resolution  space  of  heading  and  steering 
angle  dimensions  is  continued  here  for  heading  discretizations  with  32  and  80  values. 


CHAPTER 


STATE  SPACE  SAMPLING 


The  state  lattice  motion  primitives,  proposed  in  this  thesis,  have  been  motivated  in  Chapters  3  and 
4  and  found  to  be  attractive  with  respect  to  addressing  the  principal  challenges  of  motion  planning 
with  differential  constraints  (Section  1.2.2).  Chapter  5  began  an  inquiry  into  the  design  principles 
for  such  motion  primitives  by  formulating  the  search  space  design  as  an  optimization  problem  and 
providing  a  utility  function  to  be  maximized  in  this  process.  The  motivation  for  such  a  formulation 
was  to  enable  the  design  of  motion  primitives  to  be  governed  by  an  algorithm  that  would  lend 
itself  well  to  being  encoded  as  a  computer  program,  leading  to  automated  search  space  design.  As 
suggested  in  Section  1 .4,  automated  design  of  motion  primitives  is  one  of  the  contributions  of  this 
thesis. 

Now  that  the  framework  for  search  space  design  was  fixed  in  Chapter  5,  this  Chapter  begins 
the  presentation  of  the  design  principles  for  constructing  candidate  search  spaces  to  be  evaluated 
and  optimized  in  the  above  framework. 
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6.1  Dimensionality  Reduction 

In  general,  the  problem  of  selecting  the  minimal  number  of  dimensions  that  adequately  represent 
the  planning  problem  is  quite  challenging.  In  the  case  of  designing  lattice  primitives,  the  chosen 
BVP  solver  would  typically  fix  the  choice  of  dimensions.  If  solvers  for  the  same  problem  are 
available  at  different  dimensionalities,  an  iterative  dimensionality  reduction  process  may  be  under¬ 
taken  as  part  of  the  automated  search  space  design  procedure.  In  particular,  once  a  set  of  lattice 
primitives  is  designed  at  the  highest  dimensionality,  it  may  be  repeated  with  one  of  the  dimensions 
removed.  The  process  iterates  until  the  loss  of  representation  quality  exceeds  desired  application 
tolerances.  This  straight-forward  iterative  process,  though  computationally  intensive,  would  be 
able  to  reduce  the  dimensionality  of  representation  for  the  given  system. 

6.2  Design  Principles 

Once  state  dimensionality  is  fixed,  the  design  principles  for  choosing  a  good  state  sampling  rule 
in  state  lattice  context  are  given  here.  The  principles  are  purposefully  broad:  each  application 
imposes  specific  requirements  on  state  sampling.  As  suggested  in  Section  1.2.2,  no  optimal  design 
is  attempted  here  because  the  basis  for  this  optimality  is  dependent  on  a  particular  application. 
Instead,  the  presented  principles  attempt  to  arrive  at  a  design  that  addresses  the  challenges,  outlined 
in  that  section,  as  well  as  possible. 

Design  Principle  6.2.1  (Workspace  sampling).  The  sampling  of  workspace  dimensions  (2D  or  3D 
position)  is  dictated  by  the  problem  specification.  Some  of  the  criteria  that  significantly  influence 
it  include: 

•  Robot’s  perception  resolution 

•  Vehicle  footprint 

•  Control  accuracy 

These  criteria  are  to  be  adhered  to  during  motion  planner  design  since  none  of  these  criteria  are 
design  freedoms. 

□ 
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In  other  words,  when  discretizing  position  variables,  it  is  important  to  take  into  account  the 
specifics  of  the  vehicle.  For  example,  it  is  generally  helpful  to  match  the  resolution  of  planner’s 
position  discretization  to  robot’s  perception  resolution.  Making  the  former  much  finer  would  be 
somewhat  inefficient,  since  the  collision  detection  and  cost  evaluation  is  only  able  to  function  at  the 
perception  resolution.  Similarly,  planning  maneuvers  at  a  much  higher  resolution  than  the  vehicle 
footprint  and  control  accuracy  may  be  wasteful  since  the  vehicle  is  not  capable  of  executing  such 
intricate  maneuvers  in  the  first  place. 

Once  position  sampling  is  set,  the  sampling  rules  for  other  dimensions  are  derived  from  it.  The 
following  principles  are  designed  to  guide  the  development  of  these  sampling  rules. 

Design  Principle  6.2.2  (Incremental  sampling).  In  an  effort  to  minimize  the  sampling  density,  a 
design  with  the  most  coarse  sampling  rule  is  to  be  attempted  first.  The  resolution  is  to  be  increased 
only  if  the  resulting  search  space  is  determined  to  require  it. 

□ 

This  principle  is  related  to  incremental  dimensionality  reduction  described  in  Section  6.1.  For¬ 
tunately,  due  to  the  automatic  nature  of  the  motion  primitive  design,  the  incremental  sampling 
procedure  can  be  performed  with  minimal  manual  effort. 

Certain  dimensions  that  are  frequently  used  in  modeling  the  mobility  of  robots  have  a  finite 
range,  for  example  steering  angle,  velocity,  acceleration,  etc.  The  following  design  principle  sug¬ 
gests  the  minimal  sampling  of  these  dimensions. 

Design  Principle  6.2.3  (Extremal  values).  The  minimal  sampling  of  a  dimension  involves  two 
values:  the  extremals  of  the  range  of  the  dimension. 

□ 

This  principle  is  similar  to  the  bang-bang  approach  in  optimal  control  [126].  It  also  appears 
frequently  in  motion  planning  literature  [11;  28;  30;  115]. 

6.3  Natural  Sampling  Rule 

Among  similarly  performing  search  spaces,  those  with  more  coarse  sampling  are  preferred.  This 
is  an  Occam’s  razor  statement:  a  simpler  approach  is  likely  to  lead  to  a  solution  that  is  easier  to 
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develop,  test  and  maintain.  If  there  are  motions  that  require  the  least  amount  of  control  effort  for 
the  system  to  follow,  then  these  motions  are  somehow  natural  for  it.  For  example,  a  car-like  robot 
moving  on  a  flat  surface  in  a  straight  line  would  typically  require  a  low  controller  effort,  and  so  a 
straight  line  -  constant  heading  -  is  a  natural  trajectory  for  this  system.  Similarly,  moving  along  an 
arc  -  maintaining  constant  trajectory  curvature  -  is  also  a  natural  motion,  since  the  steering  angle 
joint  once  again  does  not  need  to  be  actuated  (ignoring  trajectory  following  issues  for  a  moment). 
Examples  of  these  natural  motions  are  shown  in  Figure  6.3.1. 


(a)  Ad-hoc  state  (b)  Natural  head-  (c)  Natural  curva- 
sampling  ing  ture 

Figure  6.3.1:  Motivating  natural  sampling  rule.  Part  a):  ad-hoc  state  sampling  may  result  in 
overly  complex  controls.  Straight  lines  b)  and  arcs  c)  are  natural  for  a  car-like  system  because  they 
do  not  require  actuation  of  the  steering  angle. 

Since  controls  are  induced  by  state  sampling  in  this  setting,  it  is  beneficial  to  choose  state 
samples  that  reduce  the  cost  of  controls,  e.g.  lead  to  a  greater  number  of  low-cost,  natural  motions. 

Design  Principle  6.3.1  (Natural  sampling  rule).  A  beneficial  sampling  rule  is  the  one  that  maxi¬ 
mizes  the  occurrence  of  natural  motions  in  the  resulting  control  set. 

□ 

One  of  the  methods  of  applying  this  principle  is  as  follows.  Given  the  position  sampling,  as 
specified  by  the  Principle  6.2.1  and  the  rudimentary  sampling  of  other  dimensions,  per  Principle 
6.2.3,  the  resulting  search  space  is  evaluated.  Assuming  it  is  desired  to  increase  the  sampling 
further,  a  list  of  the  dimensions  to  undergo  this  increase  is  sorted  in  terms  of  derivatives:  for 
example,  car-like  steering  angle  is  related  to  curvature  of  its  trajectory,  and  therefore  it  is  processed 
after  heading;  similarly  acceleration  is  processed  after  velocity.  With  this  ordering,  the  dimension 
undergoing  sampling  density  increase  is  taken  to  be  a  freedom,  and  the  reachability  of  the  system 
is  generated  with  an  emphasis  on  using  simplest,  natural  motions.  Finally,  statistics  are  collected 
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Natural  Heading  Discretization  (template  radius  50) 


Heading,  degrees 

Figure  6.3.2:  Natural  heading  discretization.  The  heading  values  that  correspond  to  spikes  in 
the  histogram  are  shared  by  a  greater  number  of  natural  motions  in  this  example,  and  therefore  are 
good  candidates  for  inclusion  in  sampling  the  heading  dimension,  in  the  order  of  their  frequency. 

on  the  occurrence  of  the  values  in  the  dimension  in  question;  most  frequently  occurring  values 
are  good  candidates  to  serve  as  new  samples  of  that  dimension.  Example  6.3.1  illustrates  this 
procedure. 

Example  6.3.1  (Natural  sampling  rule).  As  most  of  the  examples  in  this  thesis,  the  example  uses 
the  car-like  system  model.  Among  the  well-known  systems  with  differential  constraints,  it  is  one  of 
the  simplest  mathematically  and  quite  intuitive. 

As  described  above,  position  sampling  is  chosen  first.  For  simplicity,  a  regular  grid  in  2D 
{x,y)-space  is  chosen.  According  to  (1.2.2),  the  remaining  state  dimensions  to  sample  are  heading 
and  steering  angle. 
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First,  heading  sampling  is  chosen  using  the  procedure  above:  keeping  this  dimension  a  free¬ 
dom,  the  position  samples  were  connected  with  most  natural  motions,  straight  lines  ( no  differential 
constraints  are  considered  since  heading  is  free,  and  curvature  is  being  ignored  by  dimension  or¬ 
dering).  This  process  is  similar  to  Figure  6.3.1b).  The  procedure  is  applied  to  position  samples  in  a 
2D  ball  around  the  origin  that  is  large  enough  to  capture  the  reachability  of  the  system.  After  all  of 
the  line  motions  were  generated,  the  statistics  on  the  values  of  their  slopes  (world-frame  heading) 
are  computed  in  order  to  obtain  the  values  that  occur  most  often.  Figure  6.3.2  shows  a  histogram 
of  these  values.  The  peaks  of  the  histogram  indicate  that,  in  the  open  interval  (0°,90°),  the  most 
frequently  occurring  values  of  heading  are  arctan  of  the  ratios  \,  etc.,  corresponding  to 

45°,  26.565°,  18.435°,  14.036°,  etc.  This  observation  makes  sense  geometrically;  in  fact,  all  val¬ 
ues  in  the  histogram  must  be  arctan  of  integer  ratios.  The  ones  that  correspond  to  spikes  in  the 
histogram  are  the  most  frequent  ones,  and  therefore  are  good  candidates  for  inclusion  in  sampling 
this  dimension,  in  the  order  of  their  frequency.  Selecting  the  desired  number  of  the  heading  val¬ 
ues  (as  determined  by  testing  the  resultant  search  space  in  relevant  planning  scenarios)  concludes 
sampling  the  heading  dimension. 

Once  heading  dimension  is  sampled,  the  process  is  repeated  for  the  steering  angle  dimension. 
Once  again,  a  set  of  points,  now  three-dimensional  (x,y,  0)  since  heading  0  is  included,  in  a  region 
around  the  origin  is  chosen.  For  every  “oriented”  point  in  this  set,  a  constant-curvature  arc  is 
computed,  and  the  statistics  of  the  resulting  curvatures  is  once  again  analyzed  with  a  histogram, 
shown  in  Figure  6.3.3. 

The  following  three  figures  present  alternative  ways  of  visualizing  the  occurrence  frequency  of 
arc  curvature  values.  Figure  6.3.4  presents  a  similar  curvature  histogram  as  before,  but  it  color- 
codes  the  contribution  of  the  arcs  at  various  distances  from  the  vehicle.  This  reveals,  perhaps 
unsurprisingly,  that  the  arcs  in  the  immediate  vicinity  of  the  vehicle  contribute  most  of  the  higher 
values  of  curvature  (including  the  maximum  curvature,  the  value  that  corresponds  to  the  minimum 
turning  radius  of  the  vehicle). 

Figure  6.3.5  sorts  the  curvature  and  arc-length  values  of  the  arcs  with  respect  to  each  other.  The 
most  frequent  values  of  either  quantity  manifest  themselves  as  the  longest  horizontal  line  segments 
in  the  figure. 

Lastly,  a  2D  pseudo-color  plot  in  Figure  6.3.6  captures  both  the  relationship  of  curvature 
to  arc-length,  while  using  the  color  to  represent  frequency  of  each  value.  It  confirms  a  peak  at 


CHAPTER  6.  STATE  SPACE  SAMPLING 


83 


the  curvature  value  0.4581  that  was  also  noted  in  Figure  6.3.3.  However,  this  Figure  reveals 
that  this  curvature  value  is  not  only  abundant  in  8  dijferent  arc-lengths,  but  one  curvature-length 
combination  is  especially  dominant.  It  appears  to  be  a  good  curvature  sample  to  keep  because  it 
also  has  a  relatively  low  arc  length. 

□ 

In  conclusion,  it  is  noted  that  the  sequence  of  design  principles  presented  in  this  chapter  repre¬ 
sents  a  top-down  design  methodology.  It  also  first  caters  to  the  variables  that  are  most  constrained 
by  the  other  aspects  of  robot  design,  such  as  perception  and  controller  characteristics,  and  leaves 
the  quantities  with  the  greatest  flexibility  to  be  settled  last. 
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Curvature 


Figure  6.3.3:  Arc  curvature  histogram.  In  determining  steering  angle  discretization  for  the  car¬ 
like  system  example,  a  constant-curvature  arc  is  computed  for  each  (x,y,  0) -point  in  a  region  around 
the  vehicle.  The  curvatures  of  the  resulting  arcs  are  plotted  in  this  histogram.  The  expected  sym¬ 
metry  is  apparent.  Similar  to  Figure  6.3.2,  the  values  of  steering  angle  discretization  are  chosen 
from  the  peaks  of  the  histogram,  in  descending  order. 
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Figure  6.3.4:  Distance-sorted  curvatures.  The  histogram  shows  the  number  of  constant-curvature 
trajectories  for  each  curvature  value.  Curvature  values  are  sparse  because  the  study  in  this  exam¬ 
ple  only  computes  arcs  connecting  lattice  nodes  exactly.  The  vicinity  of  the  connected  nodes  is 
represented  by  Lattice  Radius  (LR),  equivalent  to  Li-norm  in  2D.  Note  that  arcs  from  origin  to  8 
immediate  neighbor  nodes  (magenta  bars)  provide  all  curvature  values  above  50%  of  max.  curva¬ 
ture. 
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(b)  Arc-length  sort 

Figure  6.3.5:  Arc  length  vs.  curvature.  The  eurvature  and  are-length  values  of  the  ares  are  sorted 
with  respeet  to  eaeh  other.  The  most  frequent  values  of  either  quantity  manifest  themselves  as  the 
longest  horizontal  line  segments  in  the  figure. 
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Figure  6.3.6:  Arc-length  pseudo-color.  The  red  eell  in  the  histogram  indieates  the  most  frequent 
are  eurvature-length  eombination.  It  eorresponds  to  the  peak  in  the  ID  histogram  in  Figure  6.3.3. 
Besides  its  high  frequeney  of  oeeurrenee,  this  value  of  eurvature  appears  to  be  a  good  sample  to 
keep  in  the  diseretization  beeause  it  eorresponds  to  a  relatively  low  are  length. 
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CHAPTER 


CONTROL  SET  GENERATION 


Previous  chapters  have  set  up  the  context  of  motion  planning  with  differential  constraints  using 
state  lattice  motion  primitives.  Chapter  5  began  the  discussion  of  automatic  generation  of  these 
motion  primitives  by  formulating  the  problem  of  search  space  design  as  optimization  and  setting  up 
methods  to  estimate  the  measure  of  search  space  quality,  the  objective  function  in  this  optimization. 
With  that  structure  in  place,  Chapter  6  began  actual  design  of  search  spaces  by  specifying  design 
principles  for  choosing  good  state  sampling.  Since,  in  state  lattice  setting,  that  sampling  induces 
the  sampling  of  controls,  the  resulting  search  spaces  can  already  be  used  in  planning,  although 
they  suffer  from  excessively  large  size  and  oversampling.  This  Chapter  concludes  the  discussion 
on  automatic  search  space  design  by  providing  principles  for  reducing  this  oversampling  down  to 
a  manageable  level,  while  retaining  guarantees  on  quality  of  the  resulting  search  space. 
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7.1  Approximate  Off-line  Controller  via  Reachability  Analysis 

As  was  suggested  above,  given  state  sampling  rule,  the  state  lattice  is  constructed  by  running  a 
controller  to  compute  controls  to  steer  the  system  between  all  the  pairs  of  vertices  in  the  state 
lattice  roadmap.  It  may  be  a  challenge  to  construct  such  a  controller  for  some  systems,  especially 
those  with  complex  kinematics  and  dynamics.  Fortunately,  this  controller  need  not  execute  in  real 
time  on  the  system.  This  section  suggests  one  such  controller  design  that  may  require  extensive 
computation,  yet  is  quite  general  in  its  applicability. 

The  suggestion  here  is  based  on  conventional  forward-simulation  control  sampling  techniques 
[11;  46;  60].  First,  a  dense  reachability  tree  of  the  system  was  generated  using  high-resolution, 
regular  sampling  in  control  space.  A  state  space  reachability  pruning  technique,  as  in  [11]  is 
utilized  to  maintain  computation  at  manageable  level.  Since  the  state  resolution  of  this  pruning  is  a 
multiple  of  the  desired  state  sampling  resolution,  the  endpoints  of  the  primitives  can  be  selected  to 
be  close  to  their  respective  state  cell  centers.  Those  that  are  not  sufficiently  close  are  improved  via 
gradient  descent  optimization  by  treating  the  durations  of  control  space  samples,  comprising  the 
trajectory,  as  variables  and  the  distance  of  the  end-point  to  cell  center  as  the  objective.  This  may 
be  a  significant  computation,  since  gradient  estimation  involves  repeated  execution  of  the  physics 
simulation.  The  experimental  results  presented  in  Section  8.4  utilized  this  technique,  which  led  to 
the  automated  construction  of  an  efficient  search  space  for  a  complex  dynamics  system. 

7.2  Generation  Approaches 

With  a  choice  of  state  sampling  and  a  method  of  generating  steering  functions  between  state  sam¬ 
ples,  the  capacity  to  generate  roadmap  edges  is  obtained.  The  concept  of  a  search  space  as  a 
collection  of  all  feasible  connections  between  roadmaps,  introduced  as  a  theoretical  concept,  il, 
in  Chapter  5,  is  made  concrete  here.  In  applications,  a  finite  subset  is  considered,  so  it  is  under¬ 
stood  to  be  a  set  of  all  feasible  roadmap  edges  in  a  state  space  7? -ball.  Radius  R  is  chosen  to  be  as 
large  as  possible  given  the  capacity  of  the  computing  hardware  in  order  to  obtain  the  best  possible 
representation  of  vehicle  mobility. 

A  search  space  generated  in  this  manner  is  a  complete  roadmap  design  and  is  capable  of  ad¬ 
dressing  motion  planning  queries.  However,  the  corresponding  il  is  generated  via  a  computa- 
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tionally  extensive  off-line  process,  and  its  cardinality  may  be  prohibitively  large  for  fast,  on-line 
planning.  With  this  motivation,  we  embark  on  a  concluding  discussion  in  this  thesis  on  reducing 
this  cardinality  while  minimizing  loss  of  search  space  quality. 

To  this  end,  two  types  of  control  set  generation  methods  are  proposed:  progression  and  regres¬ 
sion.  These  methods  are  further  described  below. 

7.2.1  Progression  Methods 

This  type  of  search  space  generation  forms  a  progression  of  computing  new  edges  and  adding  them 
to  the  control  set  being  developed.  Such  control  set  generation  methods  start  out  with  an  empty 
set.  Motion  primitives  are  generated  and  added  to  the  control  set  until  it  reaches  the  desired  size. 

An  example  of  this  method  is  a  technique  referred  to  as  shortest  edges,  presented  in  Algorithm 
7.1.  The  benefit  of  this  method  is  its  relative  simplicity:  unlike  the  regression  type,  it  can  be 
developed  without  elaborating  and  analyzing  large  datasets  of  state  samples  and  motion  primitives. 
However,  that  comes  at  a  cost  of  an  inability  to  provide  any  quality  guarantees  on  generated  search 
spaces.  It  is  still  possible  to  apply  the  methods  of  Chapter  5  to  validate  control  sets  for  motion 
planning  and  to  ascertain  that  they  would  produce  reasonable  results.  However,  no  statements  or 
guarantees  can  be  made  with  respect  to  the  optimization  formulation  in  (5.1.1)  because  is 

never  being  evaluated. 

Input:  representation  radius,  R 
Output:  approximating  control  set  Ea 
1  =  0; 

2  foreach  r  g  [0,7?  —  1]  do 

3  foreach  Xi  G  \  Xr,  where  Xy  is  an  R-ball  in  X  do 

4  generate  u{t)  s.t.  u{tj)  =  O,  u{tF)  =  xi, 

5  Ea=EaUu{t)-, 

6  end 

7  end 

Algorithm  7.1:  Shortest  Edges  progression 
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7.2.2  Regression  Methods 

The  regression  methods  are  signifieantly  more  involved,  but  they  are  preferred  beeause  they  sys- 
tematieally  attempt  to  retain  as  mueh  representation  quality  as  is  possible  within  the  state  lattiee 
framework.  These  methods  start  out  from  a  fully  eonneeted  roadmap  and  attempt  to  deteet  and 
prune  away  the  edges  that  are  redundant  or  otherwise  do  not  signifieantly  eontribute  to  the  quality 
of  representation.  Justifieation  for  sueh  methods  is  given  below,  and  a  design  sueeession  of  three 
speeifie  algorithms  is  presented  in  the  following  seetions,  eulminating  in  the  algorithm,  in  Seetion 
7.4.2,  that  was  the  basis  of  most  of  the  results  given  in  Chapter  8. 

By  the  given  lattiee  state  sampling  rule,  the  set  Vi  is  known.  Theorem  12. 1  develops  a  primitive 
set  Eo  that  generates  Ei,  a  superset  of  tJi,  when  used  as  the  Dijkstra’s  vertex  expansion;  free  spaee 
is  assumed  below,  unless  otherwise  noted.  More  preeisely.  Eg  is  a  set  of  primitive  sets  defined 
for  all  possible  trajeetory  initial  states  in  Vi,  up  to  the  invariant  dimensions  (e.g.  position).  For 
example,  different  values  of  heading  in  Figure  3.2.2  would  require  different  vertex  expansions;  Eg 
ean  be  viewed  as  the  union  of  the  eorresponding  primitive  sets. 

Theorem  7.2.1.  Suppose  origin  vertices  O  cVi  are  chosen  (up  to  invariant  dimensions).  Eor  every 
vertex  vi  G  Vi,  an  edge  from  each  element  ofO  to  v/  is  computed  using  the  BVP  solver  and  added  to 
Eg  (initially  empty).  When  used  as  the  Dijkstra’s  vertex  expansion,  Eg  will  search  (equivalently, 
generate)  a  Gi  such  that  Ei  D  [/;. 

Proof.  First,  by  eonstruetion,  we  eonelude  that  Eg  eontains  Vi  as  endpoints  of  its  primitives.  Using 
Eg  as  the  Dijkstra’s  vertex  expansion  amounts  to  replieating  its  edges  at  every  v,  G  Vg  If,  per 
eonneetivity  of  Ug  a  eertain  v,  eonneets  to  a  set  of  vertiees  {vj},  then  Eg,  when  replieated  at  vg 
will  eonneet  it  to  at  least  the  same  vertiees,  sinee  {vy}  C  Vg  Thus,  the  proeess  of  replieating  Eg  at 
Vi  G  Vi  generates  at  least  the  edges  present  in  tjg  and  therefore  the  indueed  set  of  edges  Ei  D  tjg  □ 

Using  Eg  as  the  vertex  expansion  represents  an  extreme  of  quality-eomplexity  trade-off.  The 
eost  of  the  eapaeity  to  explore  at  least  all  of  t//  during  seareh  is  a  very  large  branehing  faetor,  \Eg\. 
Next  we  diseuss  an  approaeh  to  manage  this  trade-off  by  eomputing  an  approximation  primitive 
set  Ea  C  Eg  of  mueh  smaller  eardinality,  while  guaranteeing  bounded  suboptimality  of  eomputable 
motions  w.r.t.  tJi  in  terms  of  arbitrary  notion  of  eost. 


CHAPTER  7.  CONTROL  SET  GENERATION 


93 


Figure  7.2.1:  Motion  decomposition.  A  motion  is  approximated  by  a  concatenation  of  two  shorter 
motions.  The  ratio  of  their  combined  cost  to  the  original  cost  is  constrained  not  to  exceed  a  user- 
specified  threshold. 


This  process  attempts  to  decompose  each  motion  in  Ei  into  two  or  more  other  motions  that  are 
also  in  Ei.  Decomposition  (Figure  7.2.1)  is  allowed  only  if  the  concatenation  of  the  components  is 
within  a  user-specified  threshold  on  cost  increase,  defined  as  a  cost  ratio  Cj  >  1  of  the  concatenated 
motion  vs.  the  original  one: 


giyp)+c{Vp,Vs)  <  Ctg{Vs)  (V.2.1) 

The  component  motions  that  can  be  reused  to  generate  other  motions  are  collected  into  Ea.  Ea 
is  designed  to  be  capable  of  generating  every  one  of  the  motions  in  Ei,  within  the  specified  cost 
increase  threshold. 

A  brute-force  approach  to  decomposing  Ei  by  enumerating  all  possible  motion  decomposi¬ 
tions  would  be  exponential  in  |.E/|  and  therefore  prohibitively  expensive.  We  propose  two  greedy 
algorithms  that  produce  near-minimal  Ea,  given  Ei  and  Cf. 

Grounds  for  eliminating  a  path: 

•  Spatial  similarity:  a  trajectory  that  is  indistinct  spatially  from  the  other  trajectory  alternatives 
(that  share  the  same  start  and  end  states)  is  removed; 

•  Relative  cost  similarity:  a  trajectory  that  is  indistinct  from  other  alternative  trajectories  in 
terms  of  its  cost  is  removed.  This  condition  is  often  faster  and  easier  to  test  than  the  previous 


one. 


CHAPTER  7.  CONTROL  SET  GENERATION 


94 


7.3  Spatial  Similarity  Regression 

An  observation  that  many  paths  in  the  system  reachability  tree  are  very  similar  leads  to  the  idea 
that  there  is  no  need  to  represent  them  explicitly  in  the  control  set.  A  straight-forward  way  to 
remove  the  redundancy  in  the  reachability  tree  is  to  eliminate  the  paths  that  are  very  similar.  A 
simple  measure  of  similarity  is  path  cross-track  difference.  An  idea  of  path  equivalence  classes 
was  proposed,  where  two  paths  were  equivalent  iff  their  cross-track  error  (along  the  whole  path) 
did  not  exceed  a  certain  value.  An  initial  approach  to  control  set  generation  replaced  all  equivalent 
paths  in  a  reachability  tree  with  a  unique  (somewhat  arbitrary)  representative  path. 

It  was  noted  that,  in  the  context  of  state  lattices,  it  is  important  to  connect  discrete  state  values 
using  paths  in  the  control  set.  Similar  to  an  equivalence  class  notion,  a  path  could  be  considered  to 
hit  a  discrete  state  value,  if  it  passes  within  £  of  it.  This  relaxation  would  introduce  small  state  dis¬ 
continuity,  but  a  judicious  choice  of  £  would  preserve  constraint  satisfaction.  Path  Decomposition 
was  proposed,  where  any  path  in  the  reachability  graph  would  be  broken  up  into  component  sub¬ 
paths  at  the  points  that  are  within  £  of  state  values  of  the  lattice.  This  by  itself  would  not  address 
the  infinite  length  of  paths;  however,  it  was  shown  that,  as  approximation,  only  the  first  segment  of 
such  decomposition  could  be  placed  into  the  control  set.  Path  Decomposition  control  sets  were  the 
first  usable  automatic  control  sets.  Control  Set  outdegree  was  controlled  via  the  £  parameter.  Using 
too  high  £  would  lead  to  degenerate  control  sets  with  less-than-useable  outdegree.  They  showed 
good  quality  of  resulting  plans  (running  A*  in  the  induced  state  lattice),  but  large  runtime  (due  to 
high  min.  outdegree  achievable). 

Even  though  the  representable  set  of  controls  Ui  is  infinite,  the  reachability  of  many  systems 
of  practical  interest  can  be  captured  well  by  analyzing  a  finite,  albeit  very  large,  subset  t//.  For 
example,  for  car-like  robots,  we  could  define  tJi  as  the  set  of  motions  that  are  contained  in  a  region 
(centered  around  the  robot)  that  is  much  larger  in  extent  than  the  robot’s  minimum  turning  radius. 
This  motion  set  will  include  many  maneuvers  that  the  robot  is  capable  of  executing,  including 
multi-point  turns  in  close  quarters.  Next  we  develop  an  explicit  and  exact  representation  of  tJi  as  a 
graph  Gi  =  ViVJEi. 
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7.3.1  Analysis  of  Path  Proximity  to  Nodes 


Once  we  reached  some  conclusions  about  the  convenient  discretization  of  the  C-space,  we  wanted 
to  verify  our  intuition  that  the  longer  the  paths  are,  the  more  likely  they  are  to  cross  other  nodes  in 
the  control  set.  We  employed  the  following  algorithm. 


1 

2 

3 

4 

5 

6 

7 

8 
9 

10 

11 

12 

13 

14 

15 


Input:  Finite  control  set  Ei,  cost  threshold  Ct 
Output:  Approximating  control  set  Ea 
=  0; 

Run  Dijkstra’s  search  with  Ei,  starting  at  0\ 
foreach  path  in  the  control  set  do 

Find  the  closest  node,  record  its  coordinates  and  distance; 

Move  along  the  path,  calculate  the  new  distance; 
if  it  is  smaller  then 
I  Continue  iterating; 
end 
else 

We  are  departing  this  node,  use  previous  distance  as  the  minimum  distance; 
Repeat  move; 

end 

Repeat  for  other  nodes  along  the  path; 

Keep  a  minimum  of  such  distance  to  any  node; 


Algorithm  7.2:  Spatial  Similarity  Decomposition. 


Consider  a  path  whose  minimum  distance  to  any  node  is  0.  This  means  that  the  path  is  perfectly 
aligned  to  a  node  along  its  way.  In  this  case  it  is  reasonable  to  eliminate  this  path  from  the  lattice 
because  it  is  composed  from  two  sub-paths,  that  must  already  exist  in  the  lattice  because  they  are 
smaller  than  the  original  path  and  by  construction  the  control  set  includes  all  paths  that  start  and 
end  on  the  control  set  nodes  and  are  within  the  control  set  radius. 

With  these  principles,  a  comprehensive  study  of  such  minimal  distances  to  any  node  has  been 
undertaken  for  paths  ending  on  control  set  perimeter.  The  results  for  a  control  set  of  angle  radius  2 
are  summarized  in  the  graph  in  Figure  7.3.2.  In  particular,  we  see  that  for  lattice  radii  of  over  10  all 
paths  come  to  within  20%  of  control  set  node  spacing.  In  other  words,  if  we  set  the  node  tolerance 
to  20%  (i.e.  the  path  “hits  the  node”  by  approaching  it  inside  this  tolerance),  then  all  of  the 
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Figure  7.3.1:  Minimum  distance  to  any  node.  Illustration  of  the  algorithm  for  calculating  the 
path’s  minimum  distance  to  any  node.  Orange  highlights  denote  Euclidean  distances  to  the  nodes 
along  the  path.  The  minimum  such  distance  is  sought. 

perimeter  paths  in  the  control  set  can  be  eliminated.  It  means  that  if  we  adopt  the  above  tolerance, 
then  the  control  set  horizon  we  have  been  seeking  is  only  10.  We  can  copy  the  control  set  of  this 
size  at  every  node  in  the  lattice  and  thereby  obtain  any  possible  motion  through  workspace. 

Also,  a  non-zero  tolerance  would  suffice  and  we  would  still  be  able  to  eliminate  all  perimeter 
paths  if  we  were  willing  to  insert  special  connection  nodes  by  choosing  convenient  for  such  nodes. 
They  would  be  the  ending  points  of  the  first  sub-paths  and  the  starting  points  of  the  second  sub¬ 
paths.  However,  inserting  extra  nodes  violates  the  regularity  of  the  control  set  and  invalidates  all 
of  its  useful  properties. 

However,  it  may  still  be  possible  to  eliminate  some  perimeter  paths  under  the  assumption  of 
a  non-  zero  tolerance  and  without  inserting  extra  nodes.  We  may  get  lucky  that  for  a  perimeter 
path,  there  exist  two  sub-paths  in  the  control  set  that,  when  concatenated,  will  generate  another 
path  that  has  the  same  initial  and  goal  poses  as  the  original  path  and  that  will  be  similar,  or  not 
spatially  distinct  from  the  original  path.  The  Lebesgue  measure  is  utilized  in  determining  the 
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Angle  radius 2 


lattice  radius:  5  to  15 


distance ,  %  spacing 


Figure  7.3.2:  Node  distance  histogram.  Summary  of  the  minimum  distanee  to  any  node  study. 
This  graph  shows  the  pereentage  of  the  paths  that  have  a  particular  value  of  minimum  distance  to 
any  node,  expressed  as  a  percentage  of  2D  spacing  between  control  set  nodes,  given  a  particular 
control  set  radius  (here  same  as  lattice  radius). 


spatial  distinctness  [84],  or  in  other  words  the  measure  of  distinctness  in  the  unsigned  area  between 
the  two  paths.  However,  unlike  in  the  first  two  path  elimination  scenarios,  here  the  existence  of  a 
pair  of  eliminating  sub-paths  is  not  guaranteed. 

Yet  another  approach  is  possible  through  understanding  the  scale  dependence  of  the  above 
discussion  of  tolerances.  Consider  an  example  where  the  size  of  the  vehicle  is  100cm,  ;  then  20% 
of  that  is  only  4cm.  Intuitively,  this  tolerance  seems  to  fall  well  within  the  precision  of  mechanical 
vehicle  control.  Thus,  if  the  path  ideally  takes  the  vehicle  4cm  off  from  the  node,  then  almost 
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certainly  the  vehicle  will  be  able  to  drive  exactly  through  the  node  (it  may  end  up  there  anyhow  due 
to  inaccuracy  of  control).  The  only  exception  is  when  the  ideal  path  already  utilizes  the  maximum 
curvature  of  the  vehicle,  in  which  case  if  we  still  split  the  original  path  at  the  node  in  question, 
then  the  vehicle  most  likely  will  end  up  slightly  off  from  the  goal  node.  As  long  as  this  final  error 
is  within  an  acceptable  tolerance  (or  the  same  mapcell),  this  maneuver  is  successful.  The  only 
caution  is  that  in  concatenating  infinitely  many  such  “inaccurate”  paths,  the  terminal  error  will 
grow  without  bound. 

7.3.2  Obtaining  a  Minimal  Set  of  Paths 

We  obtained  the  minimal  set  of  spatially-distinct  paths  by  eliminating  the  paths  that  were  not 
distinct.  In  some  fairly  large  control  sets,  we  were  consistently  able  to  eliminate  over  98%  of  paths 
(i.e.  thousands  of  paths).  Figure  7.3.3  shows  some  successful  examples  of  such  path  elimination. 

However,  as  stated  above,  the  existence  of  convenient  sub-paths  is  not  guaranteed,  and  so  it 
occurred  sometimes  that  paths  could  not  be  eliminated  with  the  above  approach.  All  of  these  cases 
seemed  to  indicate  that  the  inverse- solution  trajectory  generator  somehow  lacked  the  capability  to 
generate  the  required  sub-paths.  Sometimes  it  was  related  to  the  fact  that  the  original  path  already 
operated  at  the  limits  of  the  maximum  allowable  value  of  curvature.  Later  it  was  determined  that 
by  allowing  non-zero  curvature  at  the  connection  point,  it  was  possible  to  achieve  many  more  elim¬ 
inations.  However,  there  always  remained  at  least  one  or  two  perimeter  paths  (out  of  thousands!) 
that  still  could  not  be  eliminated. 

7.4  Cost  Similarity  Regression 
7.4.1  Leave-one-out  Regression 

This  approach  involves  elaborating  a  reachability  tree  of  the  system  and  attempting  to  reproduce 
each  of  its  edges  within  a  certain  relative  optimality  limit.  First,  the  path  in  question  is  removed 
from  the  reachability  tree.  The  remaining  reachability  tree  is  used  as  a  large  control  set  to  try  to 
reconstruct  the  original  path.  If  a  reconstruction  is  possible,  the  path  is  taken  out  of  the  control  set, 
the  next  path  is  picked  and  the  algorithm  proceeds.  In  order  to  make  sure  that  parent  vertices  are 
treated  last,  the  vertices  are  processed  in  descending  order  of  their  costs. 
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Figure  7.3.3:  Motion  decomposition  examples.  The  original  path  is  red,  the  two  eliminating  sub¬ 
paths  are  magenta  and  blue.  In  either  case,  two  sub-paths  happened  to  exist  in  the  control  set  so 
that  a),  they  could  be  concatenated  to  produce  a  path  with  the  same  initial  and  final  pose  as  the 
original,  and  b).  the  produced  path  was  not  spatially  distinct  from  the  original. 

This  algorithm  decomposes  the  motions  in  Ei  in  decreasing  order  of  their  cost.  This  implements 
a  heuristic  on  managing  the  dependencies  of  component  motions,  namely  that  the  higher-cost 
motions  are  likely  to  be  decomposed  by  lower-cost  motions.  Each  motion  Cs  to  be  decomposed  is 
selected  and  removed  from  E;.  Next,  optimal  A*  search  is  used  to  compute  a  motion  from  O  to  the 
endpoint  of  using  vertex  expansion  Ei  \  {es}-  If  the  least  cost  motion  represents  a  cost  increase 
greater  than  Ct,  the  es  is  reinserted  into  E/,  since  it  cannot  be  decomposed  satisfactorily.  Otherwise, 
it  is  left  out  of  Ei,  and  the  algorithm  repeats  by  selecting  the  next  motion  to  decompose.  Very  large 
values  of  Ct  will  lead  to  a  degeneracy  where  almost  all  motions  are  decomposed.  This  condition 
can  be  detected  by  observing  the  resulting  Ea- 

7.4.2  D*  Decomposition 

This  method  involves  generating  a  discretized,  bounded  reachability  tree.  Both  discretization  and 
bounding  cause  this  reachability  tree  to  be  an  approximation  of  the  continuum.  They  are  used  in 
order  to  make  this  process  tractable.  The  resulting  reachability  tree  is  used  as  a  large  control  set 
in  growing  another  reachability  tree  of  similar  size  using  Dijkstra’s  (best-first)  search.  This  allows 
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generation  of  multiple  ways  to  attain  the  nodes  in  the  second  reachability  tree.  The  different  ways 
of  attaining  each  node  are  analyzed  and  only  one  way  is  selected  such  that  it  is  still  within  the 
desired  relative  optimality  and  the  size  of  the  resulting  control  set  is  minimized.  Ensuring  the 
satisfaction  of  the  relative  optimality  limit  is  done  by  explicitly  considering  predecessor’s  costs 
using  the  formula: 

p  =  min(p,,— ) 
a  —  c 

where  (3j  is  the  desired  (maximum)  relative  optimality  threshold,  |3  is  the  updated  threshold  of  the 
predecessor  vertex,  a*  is  the  known  optimal  cost  of  a  vertex,  a  is  the  cost  of  the  path  that  goes 
through  the  given  predecessor  vertex,  and  c  is  the  cost  of  the  edge  between  the  predecessor  and  the 
given  vertex. 

The  previous  algorithm  has  a  disadvantage  in  complexity:  it  requires  running  A*  “from  scratch” 
\Ei  I  times,  in  graphs  with  large  (albeit  reducing)  branching  factors.  Inspired  by  the  capacity  of  in¬ 
cremental  search  algorithms,  e.g.  D*  Lite  [71],  to  prevent  repetitive  “from  scratch”  search  by  virtue 
of  reusing  computation,  we  propose  an  alternative  with  much  better  runtime  (Algorithm  7.3).  It 
does  a  single  Dijkstra’s  elaboration  of  Eq  vertex  expansion  to  explore  the  extent  of  tJi  (line  2) 
and  then  performs  the  decomposition  analysis  from  Section  7.4.1  while  reusing  the  collected  ver¬ 
tex  predecessors,  similar  to  evaluating  the  r/z5-value  (4.2.1).  In  this  manner,  unlike  the  traditional 
application  in  planning,  the  principles  of  the  D*  algorithm  are  used  here  for  search  space  design. 

Once  the  predecessors  are  computed,  only  those  that  can  possibly  yield  Cf-decompositions  are 
retained  (lines  3-7).  If  a  state  has  only  a  single  predecessor,  it  implies  by  construction  that  the  one 
and  only  motion  that  connects  it  to  the  origin  is  the  original  edge  in  Ei,  and  motion  decomposition 
cannot  succeed.  This  edge  is  entered  into  Ea-  Since  the  motions  that  form  decompositions  may  be 
further  decomposed  themselves,  we  keep  track  of  per-vertex  cost  thresholds  with  a  database  db', 
thresholds  for  all  vertices  are  initially  set  to  Ct  on  line  5. 

Line  8  follows  the  cost-sorting  heuristic  discussed  in  Section  7.4.1.  The  next  line  selects  edges 
in  decreasing  order  of  cost  of  their  destination  endpoints.  Lines  10-12  re-evaluate  the  number  of 
admissible  predecessors.  Line  13  enumerates  the  options  for  decomposing  the  edge  Cs  leading  to 
Vs.  The  algorithm  attempts  to  select  the  decomposition  that  is  likely  to  minimize  the  final  Ea.  To 
this  end,  it  uses  two  other  heuristics.  The  first  one  is  sorting  potential  decompositions  in  increasing 
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Input:  finite  control  set  Ei,  cost  threshold  Ct 
Output:  approximating  control  set  Ea 
1  =  0; 

2  Run  Dijkstra’s  search  with  Ei,  starting  at  O; 

3  foreach  es  G  Ei  do 

4  PredcXvs)  =  {vp:  s.t.  givp)  +  c{vp,Vs)  <  Og(v^)}; 

5  db{vs)^Ct\ 

6  if  \Predc,{ys)  \  =  1  then 

7  I  Ea  =  Ea  U  Cs', 

8  end 

9  end 

10  Esort  =  sort(£/); 

11  foreach  Cs  G  Eson  do 

12  if  \PredcXvs)  \  =  1  then 

13  Ea  =  Ea  U  6s, 

14  continue; 

15  end 

16  foreach  Vp  G  sort{PredcXvs))  do 

17  if  Cvp,v,  G  Ea  then 

18  adjust_threshold(vp,  v^); 

19  goto  9; 

20  end 

21  end 

22  v*=  argmin  g(vp) +c(vp,v^); 

VpePredc,{vs) 

23  adjust_threshold(v*,  v^); 

34  Ea  =  Ea  U 

25  end 

Algorithm  7.3:  D*  Decomposition. 

order  of  cost  (sort  on  line  13),  in  an  attempt  to  choose  a  decomposition  with  cumulative  cost  that  is 
as  close  as  possible  to  the  original  motion.  The  second  heuristic  is  preferring  a  decomposition,  in 
which  one  of  the  segments  is  already  found  in  Ea  (lines  14-16),  in  an  attempt  to  avoid  adding  new 
elements  to  Ea- 

Once  an  edge  is  decomposed  into  two  segments  connecting  at  vertex  Vp,  the  segment  leading 
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Input:  vertices  Vp  and 

Output:  db{vp),  Predc,{vp),  and  Esori  modified 

1  0C=  (^(vp)+c(vp,v,))/^(Vi); 

/  ^  db{v,)g{vs)-c{vp,vd  . 
t  ag(v,)-c{vp,vs)  ’ 

3  if  Cj  <  db{vp)  then 

4  db{vp)=c'j\ 

5  PredcXvp)  =  {v'p,  s.t.  g{v'p)  +  c{v'p,Vp)  <  db{vp)g{vp)]\ 

6  if  g{vp)  >  ^(Vi)  then 

I  Esort  —  Esort  U 

8  end 

9  end 

Algorithm  7.4:  adjust.threshold  function. 

to  Vp  may  in  turn  be  decomposed;  however  the  cost  threshold  for  its  decomposition  may  have  to 
be  different  from  q.  To  see  this  reasoning,  suppose  path  length  is  taken  as  cost  (Figure  7.2.1). 
Using  the  cost  sorting  heuristic,  the  longer,  original  motion  (terminating  in  v^)  will  be  decomposed 
before  the  first  segment  of  decomposition  (terminating  in  Vp).  When  the  latter  is  in  turn  selected 
for  decomposition,  we  rewrite  (7.2.1)  as: 

Ctgiyp)+c{yp,Vs)  <  ctgivs)  (7.4.1) 

Depending  on  the  value  of  c(vp,v^),  the  required  cost  increase  threshold  c[  for  the  recursive 
decomposition  of  Vp  may  be  c[  <  Ct.  Thus,  as  soon  as  a  decomposition  relationship  is  established 
between  the  vertices,  the  necessary  reduction  of  cost  threshold  of  the  predecessor  vertex  is  com¬ 
puted  with  the  function  adjust  Jhreshold.  If  the  algorithm  gets  to  line  17,  no  decomposition  oppor¬ 
tunities  have  been  selected  by  the  heuristics.  This  line  selects  a  decomposition  with  the  least-cost 
predecessor;  it  is  added  to  Ea  on  line  19. 

Algorithm  7.4  presents  the  adjust  Jhreshold  function.  In  line  2,  it  computes  c[,  the  new  cost 
threshold  for  the  predecessor  vertex  Vp.  If  this  value  is  less  than  the  previous  one,  it  is  recorded 
on  line  4,  and  the  list  of  admissible  predecessors  is  reviewed  on  line  5.  Lines  6-7  address  the  case 
where  the  cost  sorting  heuristic  is  incorrect,  and  the  predecessor  Vp  has  equal  or  greater  cost  than 
Vs-  In  this  case,  any  changes  done  to  Vp  need  to  be  propagated  to  its  potential  predecessors,  so  the 
next  iteration  of  Algorithm  7.3  must  select  Vp’s  edge,  Cp  (line  3). 


CHAPTER 


EXPERIMENTAL  RESULTS 


8.1  Validating  State  Lattice  Motion  Primitives 

A  set  of  experiments  was  eondueted  in  order  to  assess  the  performance  of  state  lattice  motion  prim¬ 
itives  in  comparison  with  popular  types  of  motion  primitives  in  motion  planning  with  differential 
constraints.  These  experiments  invoke  the  planner,  based  on  such  primitives,  with  a  number  of 
queries  in  a  variety  of  configurations.  Here  we  compare  our  planner  with  other  algorithms:  the 
basic  grid-based  planner  (on  4-,  8-,  and  16-connected  grids)  and  the  well-known  Barraquand  and 
Latombe  (BE)  nonholonomic  planner  [9].  In  doing  so,  we  attempted  to  level  the  playing  field  for 
fair  comparison.  Therefore,  the  same  implementation  of  A*  was  used  for  all  algorithms.  The  pri¬ 
mary  differences  were  in  the  node  expansions,  and  the  cost  and  heuristic  estimates.  For  the  former, 
we  have  adopted  a  notion  of  a  generalized  control  set:  for  the  grid  search  it  is  simply  a  grid-based 
node  expansion  (straight  paths),  whereas  for  the  BE  planner  it  is  the  node  expansion  as  described  in 


CHAPTER  8.  EXPERIMENTAL  RESULTS 


104 


that  work.  The  first  set  of  tests  looks  at  how  the  sample  lattice  control  set  performs  in  comparison 
to  other  types  of  generalized  control  sets.  Next  the  impact  of  the  heuristic  look-up  table  on  state 
lattice  performance  is  examined. 


8.1.1  Experimental  Setup 


A  representative  lattice  control  set  was  used  in  all  tests.  Its  state  space  consisted  of  the  2D  trans¬ 
lational  coordinates,  heading  and  curvature  (jc,y,  0,k).  For  the  sake  of  simplicity,  curvature  was 
constrained  to  be  zero  at  each  discrete  state.  This  control  set  is  depicted  in  Figure  8.1.1. 


Figure  8.1.1:  Experiment  control  sets.  The  state  lattice  control  set  selected  for  testing  has  16 
discrete  headings,  a  maximum  curvature  of  1/8  of  cell,  and  an  average  outdegree  of  12,  for  a  total 
of  192  curves.  The  straight  edges  cannot  be  seen  because  they  are  obscured  by  the  longer  curved 
edges. 

For  these  tests,  a  list  of  10,000  random  queries  was  generated,  consisting  of  an  initial  and  final 
state,  also  expressed  as  position,  heading,  and  curvature.  The  set  of  queries  was  generated  with 
the  intent  to  require  the  planner  to  produce  paths  ranging  from  simple  (nearly  straight  paths)  to 
complex  (e.g.  parallel  parking  or  n-point  turn  maneuvers)  among  obstacles.  Each  query  was  tested 
with  a  wide  variety  of  configurations,  including  different  obstacle  fields,  alternative  control  sets 
such  as  a  grid  or  Barraquand-Latombe,  and  A*  heuristic  functions. 

Start  and  goal  states  were  produced  with  a  random  number  generator  that  provides  evenly 
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distributed  real  values  in  a  requested  range.  Initial  positions  were  selected  at  random  from  the  free 
space  in  order  to  produce  the  maximum  possible  variety  of  queries.  The  goal  position  was  then 
specified  by  a  randomly  selected  radius  and  angle  specified  in  polar  coordinates  with  respect  to 
the  start,  repeating  this  step  as  necessary  to  ensure  that  the  goal  is  also  in  the  free  space.  Initial 
and  final  headings  were  randomly  selected,  and  curvature  at  end-points  was  constrained  to  be  zero. 
Goal  states  were  constrained  to  be  no  more  than  ten  minimum  turning  radii  (80  cells)  from  their 
corresponding  start  states  when  projected  onto  x,y  space. 

Each  query  was  tested  in  two  worlds.  In  both  worlds,  cost  to  traverse  free  space  was  held 
constant  at  1  unit  per  cell  of  free  space.  Hence,  path  cost  was  equal  to  the  distance  traveled.  The 
baseline  case  was  an  obstacle-free  world,  meaning  that  the  HLUT  gave  exact  solutions  for  cost. 
Results  were  also  obtained  using  a  world  with  randomly  placed  point  obstacles,  shown  in  Figure 
8.1.2  with  paths  generated  by  two  different  planners.  These  obstacles  are  the  size  of  one  map 
cell,  which  in  this  control  set  corresponds  to  l/8th  of  the  minimum  turning  radius.  Points  were 
generated  with  uniform  distribution  and  5%  density  in  the  plane. 


Figure  8.1.2:  World  with  Obstacles.  A  portion  of  the  world  with  point  obstacles  used  in  the 
experiments  is  shown  here.  Two  plans  are  shown  which  solve  the  same  query.  The  black  line 
shows  the  plan  generated  by  the  state  lattice,  while  the  grey  line  traces  the  path  returned  from  the 
BF  planner. 

Metrics  for  performance  included  time,  memory  consumption,  and  the  quality  of  the  resulting 
plan.  In  analyzing  the  performance  of  the  planner,  it  is  necessary  to  distinguish  among  the  spectrum 
of  queries  ranging  from  simple  to  complex,  as  was  suggested  above.  In  the  case  of  a  grid.  Euclidean 
distance  would  be  an  appropriate  measure  of  difficulty.  When  planning  in  full  state  space,  however, 
the  length  of  the  path  followed  by  a  nonholonomic  vehicle  can  vary  widely  even  in  cases  where 
Euclidean  distance  between  the  end-points  is  held  constant. 

In  order  to  quantify  the  complexity  of  a  particular  query,  the  distinction  is  made  between  ab- 
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solute  difficulty,  which  is  proportional  to  path  length,  and  relative  difficulty,  which  reflects  how 
much  the  resulting  path  deviates  from  a  straight  line.  Figure  8.1.3  illustrates  the  two  concepts. 
Both  factors  contribute  to  the  overall  resource  requirements  for  a  particular  planning  problem.  Ab¬ 
solute  difficulty  is  measured  here  by  the  path  length.  In  the  case  of  relative  difficulty,  the  ratio  of 
Euclidean  distance  /  nonholonomic  distance  was  used.  In  this  scale,  values  near  one  mean  that  the 
resulting  path  is  nearly  a  straight  line,  while  those  values  nearest  to  zero  indicate  that  much  ma¬ 
neuvering  is  necessary  to  reach  the  final  pose.  In  the  analyzes  below,  results  are  shown  for  queries 
with  absolute  difficulty  of  40  cells.  This  number  was  chosen  arbitrarily  for  clarity  of  presentation. 
Any  other  absolute  difficulty  would  have  conveyed  similar  results. 


Figure  8.1.3:  Absolute  and  Relative  Query  Difficulty.  The  difficulty  of  a  query  can  be  quantified 
in  two  dimensions.  Each  path.  A,  B,  and  C,  starts  at  O.  Query  A  is  high  in  absolute  difficulty  as 
well  as  relative  difficulty  because  it  is  long  and  has  multiple  cusps.  B  is  simple  in  both  measures. 
Query  C  has  the  same  absolute  difficulty  as  A  (same  length),  but  the  same  relative  difficulty  as  B 
(nearly  a  straight  line). 

In  the  sequel,  a  variety  of  alternative  control  sets  are  considered.  In  each  set  of  cases,  the 
same  framework  is  used,  including  an  A*  planner,  heuristics,  and  world  representation.  The  only 
distinction  is  the  method  by  which  neighboring  states  are  generated  during  the  expansion  step.  A 
standard  lattice  control  set,  depicted  in  Figure  8.1.1,  was  used  across  all  tests.  An  overview  of 
all  the  control  sets  which  are  considered  here  is  presented  in  Table  8.1.  Conceptually,  a  control 
set  with  longer  edges  requires  fewer  expansion  steps  to  reach  the  goal  but  a  greater  total  length 
to  integrate,  proportional  to  the  outdegree.  Correspondingly,  a  higher  outdegree  increases  mem¬ 
ory  requirements  for  the  planner  and  computation  in  the  expansion  step,  but  also  improves  the 
chance  of  finding  a  more  direct  route  in  fewer  plan  steps.  Below,  the  best  performing  of  several 
combinations  is  empirically  determined. 
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Control  Set 

Total  Edges 

Ave.  Edge  Length 

Ave.  Outdegree 

original  control  set 

192 

8.72 

12 

Ctrl  set  w/  turn-in-place 

224 

7.47 

14 

BL 

96 

4.00 

6 

grid-4 

4 

1.00 

4 

grid- 8 

8 

1.21 

8 

grid- 16 

16 

1.72 

16 

Table  8.1:  A  quantitative  overview  of  control  sets.  Parameters  of  a  control  set  have  a  strong 
influence  on  how  the  planner  will  perform  while  using  them. 


8.1.2  Comparison  TO  A  Grid  Planner 

The  basic  grid  search  method  of  planning  does  not  consider  heading,  which  leads  to  several  draw¬ 
backs.  First,  it  is  not  clear  what  the  heading  state  is  unless  two  adjacent  states  on  the  path  are 
compared  and  even  in  this  case,  the  heading  can  only  have  4  values.  Second,  it  is  impossible  to 
plan  feasible  paths  for  vehicles  incapable  of  a  point  turn.  Third,  heading  continuity  constraints 
cannot  be  enforced.  A  grid  has  nonetheless  been  widely  used  due  to  its  simplicity  and  efficiency. 

But  is  this  approach  inherently  faster  than  searching  in  a  full-dimensional  state  space?  For  this 
test,  the  planner  was  run  with  four  different  control  sets,  in  each  case  using  a  heuristic  function 
which  always  returned  the  exact  distance  to  the  goal.  The  basic  state  lattice  was  matched  with  a 
large  HLUT.  Three  grid  control  sets  were  tested,  in  which  each  state  is  connected  to  its  4,  8,  and 
16  nearest  neighbors  (Figure  8.1.4),  using  a  perfect  heuristic  for  each  level  of  connectivity. 


•  •  •  •  • 


Figure  8.1.4:  Grid  Control  Sets.  Three  different  grid  control  sets  were  tested.  A  point  is  connected 
to  the  4,  8,  or  16  nearest  neighbors  that  have  unique  headings. 

Performance  of  the  control  sets  in  the  absence  of  obstacles  is  shown  in  Figure  8.1.5.  In  order  to 
normalize  the  results,  only  queries  of  absolute  difficulty  equal  to  40  cells  were  used.  The  data  are 


CHAPTER  8.  EXPERIMENTAL  RESULTS 


108 


presented  across  a  range  of  relative  difficulty.  In  the  absence  of  obstacles,  very  interestingly,  the 
lattice  performs  on  par  with  basic  grid  search.  Only  on  most  difficult  queries,  the  lattice  consumes 
more  CPU  time  than  a  grid  control  set  because  the  nonholonomic  path  solution  diverges  more 
dramatically  from  a  straight  line  solution.  Of  course,  for  more  difficult  problems,  the  answer 
returned  by  the  grid  is  increasingly  infeasible  to  execute  on  a  real  vehicle  -  and  the  path  is  also 
unlikely  to  arrive  at  the  correct  heading. 


Figure  8.1.5:  Lattice  vs.  Grid  without  Obstacles.  Queries  with  an  absolute  difficulty  of  40  cells 
are  shown.  The  lattice  performs  similarly  to  the  grid  planners  overall.  Only  for  the  greatest  relative 
difficulty  (nearest  zero)  does  the  lattice  require  more  CPU  cycles.  Of  course,  the  benefit  of  this 
extra  computation  is  a  plan  which  is  feasible. 

Results  in  the  presence  of  obstacles  are  shown  in  Figure  8.1.6.  It  is  intuitive  that  the  grid  should 
outperform  the  lattice  in  this  obstacle  field  for  any  class  of  query.  If  an  obstacle  appears  in  the  path 
of  a  grid  plan,  that  plan  is  often  displaced  by  only  a  few  cells  in  order  to  circumvent  the  obstacle.  In 
the  case  of  the  lattice  under  test,  however,  only  smooth  continuous  paths  with  a  maximum  curvature 
of  1/8  =  0. 125  are  considered.  These  requirements  substantially  limit  the  planner’s  options,  making 
it  more  difficult  to  find  a  satisfactory  path  through  an  obstacle  field  as  shown  in  Figure  8.1.7.  So 
while  the  grid  path  deviates  slightly  in  the  cluttered  environment,  paths  generated  by  the  state 
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lattice  are  often  much  more  sophisticated  in  order  to  plan  around  obstacles.  Thus,  the  difficulty 
experienced  by  the  lattice  planner  reflects  the  true  mobility  limits  of  the  vehicle.  In  plain  terms, 
a  grid  planner  produces  faster  answers,  but  they  are  usually  wrong.  Despite  the  increased  search 
requirements,  the  lattice  remains  consistently  only  one  order  of  magnitude  slower  than  the  grid 
planner,  returning  on  average  in  less  than  0. 1  second  for  all  classes  of  query. 


Rebdte  Diffbi kf  ofEuclidegn  /5onhobno(nbDitanc«) 


Figure  8.1.6:  Lattice  vs.  Grid  with  5%  Obstacle  Density.  Queries  of  length  40  cells  are  shown. 
The  grid  outperforms  the  lattice,  but  it  usually  returns  infeasible  solutions  in  a  dense  obstacle  field. 
Lattice  planner  runtime  is  still  acceptably  fast. 


Figure  8.1.7:  Obstacle  Avoidance  with  Two  Control  Sets.  Grid  planners  can  easily  avoid  small 
isolated  obstacles,  as  shown  by  the  black  path.  By  contrast,  the  grey  path  has  limited  curvature  and 
so  can  be  feasibly  traversed  by  a  constrained  vehicle. 
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8.1.3  Comparison  to  Full  C  Space  and  Nonholonomic  Planners 

The  comparison  between  the  state  lattice  and  grid-based  planner  is  not  entirely  fair,  since  the  two 
planners  search  different  spaces,  and  the  grid  results  cannot  be  traversed  by  nonholonomically 
constrained  vehicles  without  post-processing.  The  Barraquand  and  Latombe  (BL)  planner  is  well 
known  and  has  been  a  popular  nonholonomic  planner  for  over  a  decade.  It  has  also  been  used  in  a 
variety  of  real  mobile  robot  applications  [7],  [96].  In  this  section,  several  different  implementations 
of  that  planner  are  examined.  In  all  cases,  the  identical  A*  algorithm  was  applied,  but  the  heuristic 
cost  estimate  was  altered  to  produce  a  fair  comparison.  In  the  first  case,  the  algorithm  was  run  as 
documented  in  [9],  with  a  zero  heuristic  applied.  In  the  second  case.  Euclidean  distance  was  used 
as  heuristic.  Finally,  an  attempt  was  made  to  produce  a  perfect  heuristic  by  generating  an  HLUT 
for  the  BL  planner,  for  a  fair  comparison  with  an  HLUT-enabled  lattice  planner. 

In  order  to  make  the  BL  control  set  work  in  a  fashion  compatible  with  the  state  lattice  test 
framework,  an  adjustment  was  necessary.  Barraquand  and  Latombe  [9]  describe  the  edge  lengths 
as  being  the  diameter  of  a  cell  and  assert  that  their  planner  requires  the  discretization  of  the 
search  parameters  to  be  fine  enough.  The  state  lattice’s  discretization  settings  were  experimentally 
adjusted  to  allow  proper  operation  of  the  BL  planner:  the  edge  lengths  were  set  to  four  cell  widths. 
The  resulting  reachability  tree  is  shown  in  Figure  8.1.8  along  with  the  standard  state  lattice  tree. 
Reverse  edges  were  omitted  from  both  trees  for  clarity. 

The  performance  of  BL  with  three  different  heuristics  is  shown  in  Figure  8.1.9  along  with 
the  state  lattice  using  two  different  heuristics.  The  heuristic  which  enforces  optimality  for  the  BL 
planner  is  zero,  as  proposed  by  authors,  but  the  computational  time  required  makes  this  impractical 
in  most  situations.  In  a  fair  match-up  using  the  Euclidean  distance  heuristic,  the  two  planners 
perform  comparably,  but  only  the  lattice  retains  optimality  (Figure  8.1.10).  However,  note  that  the 
Euclidean  heuristic  was  used  for  the  lattice  planner  only  for  the  fairness  of  the  comparison.  The 
lattice  planner  using  the  HLUT  significantly  outperforms  our  implementation  of  BL,  both  with  and 
without  its  own  HLUT,  as  shown  below. 

The  HLUT  generation  algorithm  was  run  using  a  BL  control  set  in  an  attempt  to  produce  a 
perfect  BL  heuristic.  However,  in  A*  runs  which  used  this  heuristic,  many  false  leads  were  still 
explored.  The  HLUT  generation  algorithms  discussed  above  make  a  fundamental  assumption  that 
the  cost  between  two  points  in  the  graph  depends  only  on  their  relative  states  because  the  lattice  is 
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Figure  8.1.8:  Reachability  Trees  for  the  Lattice  and  BL.  At  top,  the  first  1400  expansions  of 
the  basic  state  lattice  control  set  in  best-first  order.  At  bottom,  the  first  1400  expansions  of  the  BL 
control  set  were  generated  using  the  same  algorithm.  Lighter  edges  are  older  in  both  images. 


regular  in  translational  discretization.  The  BL  algorithm,  however,  does  not  operate  on  a  regular 
search  space.  As  edges  grow  outward,  other  edges  are  excluded,  such  that  the  path  between  two 
points  depends  on  the  order  in  which  states  were  expanded  to  produce  each  edge  between  the  two 
points.  So  while  using  the  HLUT  resulted  in  the  best  BL  performance,  this  heuristic  cannot  be 
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Figure  8.1.9:  Lattice  vs.  BL  without  Obstacles.  Queries  with  an  absolute  difficulty  of  40  cells 
are  shown.  Various  heuristics  are  considered  for  each  planner.  Only  when  the  slow  Zero  heuristic 
is  used  does  BL  return  optimal  paths. 


Figure  8.1.10:  BL  sub-optimality  example.  In  black,  the  BL  planner  with  Euclidean  heuristic  is 
used.  In  grey,  the  standard  lattice  control  set  with  Euclidean  heuristic  returns  an  optimal  path.  BE 
does  not  because  it  does  not  permit  revisiting  of  states. 


guaranteed  to  be  admissible  for  that  algorithm.  Note,  however,  that  with  any  non-zero  heuristics 
the  planner  does  not  guarantee  optimality  anyway. 

An  alternative  lattice  was  also  tested.  This  control  set  has  the  same  controls  as  the  basic  lattice. 
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Figure  8.1.11:  Control  sets  with  and  without  turn-in-place.  In  black,  a  control  set  with  a  mini¬ 
mum  turning  radius  of  8  cells  is  used.  In  grey,  the  same  control  set  is  augmented  by  two  additional 
controls  which  allow  the  vehicle  to  turn  in  place  at  a  cost  equal  to  traversing  5  cells.  This  plan 
performs  the  tum-in-place  maneuver  twice  in  order  to  execute  a  sharp  turn. 
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Figure  8.1.12:  Lattice  vs.  enhanced  lattice  without  obstacles.  Queries  with  an  absolute  difficulty 
of  40  cells  are  shown.  Performance  for  the  two  lattices  is  comparable. 

except  that  extra  zero-length  edges  were  added  so  that  the  vehicle  is  permitted  to  turn  in  place.  The 
cost  of  these  additional  edges  was  computed  based  on  the  aggregate  distance  of  travel  of  each  of  the 
wheels  in  the  vehicle.  The  effect  of  this  capability  on  resulting  plans  is  depicted  in  Figure  8.1.11, 
where  the  path  is  substantially  shortened  by  avoiding  circuitous  maneuvering.  These  additional 
controls  have  no  significant  effect  on  overall  planner  performance  (Figure  8.1.12),  but  they  provide 
valuable  added  flexibility  in  negotiating  dense  obstacle  fields.  Moreover,  for  practical  applications, 
the  lattice  planner  algorithm  allows  tuning  the  robot’s  preference  for  performing  point  turns  versus 
smooth  maneuvers  by  assigning  appropriate  costs  to  the  zero-length  edges. 
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8.2  Graduated  Fidelity 

In  order  to  evaluate  the  effects  of  graduated  fidelity,  we  performed  simulated  and  field  experiments 
with  mobile  robots.  In  all  experiments,  a  planetary  rover  prototype  roughly  0.8m  by  1.0m  in  size 
was  used.  It  was  assumed  to  be  a  nonholonomic  vehicle  capable  of  moving  forward  and  back  with 
minimum  turning  radius  0.5m.  The  vehicle  was  capable  of  point-turns,  although  such  maneuvers 
were  considered  costly  and  used  as  a  last  resort.  The  search  space  consisted  of  two  subgraphs: 
a  high  fidelity  subgraph  that  moved  with  the  vehicle  and  a  low  fidelity  subgraph  elsewhere.  The 
(.r,y)  resolution  of  both  subgraphs  featured  square  cells  of  20  cm.  The  average  outdegree  of  the  4D 
high  fidelity  subgraph  was  45  (a  state  lattice).  The  outdegree  of  the  2D  low  fidelity  subgraph  was  8 
(grid  connectivity).  The  dimensions  of  the  state  lattice  included  the  translational  coordinates  (.r,y), 
discretized  as  a  grid,  as  well  as  heading  and  curvature.  The  planner  used  the  Euclidean  distance 
heuristic,  and  an  improvement  in  performance  can  be  expected  with  better  informed  heuristics. 

The  set  of  experiments  below  attempts  to  quantify  the  effects  of  the  size  of  the  high  fidelity 
subgraph  around  the  vehicle  on  both  runtime  performance  and  the  quality  (cost)  of  the  solution. 
The  second  set  of  simulated  and  real  vehicle  experiments  demonstrates  the  performance  of  the 
planner  in  a  realistic  setting. 

The  plot  in  Figure  8.2.1  reports  the  results  of  the  first  type  of  experiments.  The  planner  used 
an  environment  with  random  single  map  cell  obstacles  independently  and  identically  distributed 
with  probability  of  3%.  This  is  the  same  environment  as  used  later  in  the  second  set  of  simulated 
experiments,  and  it  is  illustrated  in  8.3.1.  To  avoid  confounding  the  results  with  simulated  per¬ 
ception,  the  cost  map  was  fully  known  a  priori.  For  each  experiment,  the  robot  was  placed  at  a 
random  location  in  its  environment,  and  the  goal  was  chosen  approximately  100  cells  away.  The 
robot  then  navigated  toward  the  goal,  while  periodically  replanning  after  traversing  approximately 
two  cells.  The  replanning  runtime  and  the  costs  of  traversed  edges  were  accumulated.  The  runtime 
was  computed  as  (real-time)  seconds  elapsed  for  each  planning  computation.  The  experiment  was 
repeated  using  a  different  size  of  the  high  fidelity  subgraph  around  the  robot.  Both  the  runtime 
and  cost  measurements  were  normalized  by  dividing  by  the  respective  quantity,  featured  by  the 
lattice  planner  without  graduated  fidelity  (high  fidelity  throughout).  The  plot  suggests  that  using 
the  high  fidelity  subgraph  of  a  smaller  size  appears  to  offer  a  significant  performance  improvement 
with  a  very  small  increase  in  traversal  cost.  For  larger  sizes  of  the  subgraph,  the  runtime  increases 
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significantly.  This  is  expected  in  the  given  seenario,  where  the  subgraph  around  the  vehiele  grows 
radially  outward.  Larger  values  on  the  x-axis  represent  larger  diameters  of  this  subgraph.  The 
inereased  eireumferenee  eauses  signifieantly  larger  numbers  of  perimeter  vertiees  to  be  proeessed 
to  reeonneet  the  subgraphs.  The  plot  demonstrates  that  beyond  a  eertain  size  of  the  movable  sub¬ 
graph  (here  about  20%  of  the  distanee  to  the  goal),  the  runtime  eosts  of  modifying  the  seareh  spaee 
exeeed  its  runtime  benefits.  In  this  applieation,  the  approaeh  is  mostly  helpful  for  smaller  sizes  of 
the  subgraph:  for  a  size  of  10%  of  the  distanee  to  the  goal,  the  runtime  benefit  is  almost  an  order 
of  magnitude.  Note  that  in  most  realistie  settings,  the  pereeption  horizon  (whieh  would  typieally 
prompt  high-fidelity  representation)  is  typieally  mueh  less  than  10%,  and  the  motion  goal  of  the 
robot  may  be  very  far,  perhaps  kilometers  away.  Differentially  eonstrained  planning  at  high  fidelity 
would  not  be  feasible  at  sueh  seale. 

The  graduated  fidelity  trials  are  somewhat  at  a  disadvantage  in  the  study  above,  sinee  a  robot 
using  high  fidelity  throughout  requires  no  replanning,  given  the  assumed  known  environment.  In 
a  realistie  setting,  replanning  may  still  be  required  due  to  pereeption  updates.  If  we  ehoose  the 
settings  from  the  simulated  robot  navigation  experiments,  where  the  pereeption  is  limited  to  a 
realistie  radius  around  the  vehiele,  the  benefits  of  varying  fidelity  are  more  pronouneed,  as  shown 
in  Figure  8.2.2.  For  extremely  large  sizes  of  the  high  fidelity  subgraph  (over  40%  of  the  distanee 
to  the  goal),  the  runtime  eost  is  signifieant  due  to  repairing  eonneetions  for  a  large  number  of 
perimeter  vertiees.  However,  as  argued  above,  mueh  smaller  sizes  of  high  fidelity  subgraphs  are 
meaningful  for  real  applieations. 

8.3  Autonomous  Navigation 

Here  we  present  the  results  of  experiments,  simulated  and  real,  that  demonstrate  planner  perfor- 
manee  in  a  realistie  setting,  where  the  robot  moves  in  a  challenging  environment  toward  a  distant 
target.  In  the  simulated  experiment,  the  robot  has  a  pereeption  region  limited  to  21x21  eells,  een- 
tered  around  the  robot.  No  pereeption  information  is  available  outside  this  horizon.  The  size  of 
the  high  fidelity  region  is  the  same  size  as  the  pereeption  region.  Otherwise,  the  setup  is  the  same 
as  above.  For  elarity.  Figure  8.3.1  shows  a  40-meter  subset  of  a  500-meter  path,  traversed  in  this 
setting.  Grey  eells  are  obstaeles  that  have  not  yet  eome  into  view  of  the  robot  and  are  unknown  to 
it.  Blaek  cells  (and  red  eells  in  the  insets)  are  obstaeles  that  were  seen  by  the  robot.  The  dark-gray 
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Figure  8.2.1:  Simulated  robot  traversal.  The  results  of  the  simulated  robot  traversal  using  grad¬ 
uated  fidelity.  The  horizontal  axis  is  the  percentage  of  the  search  space  (between  robot  and  goal) 
occupied  by  the  high  fidelity  subgraph.  For  example,  10%  denotes  a  high  fidelity  subgraph  of 
radius  10  cells  around  the  robot,  given  that  distance  to  the  goal  is  100  cells.  The  vertical  axis  is 
the  ratio  of  cumulative  runtime  and  traversal  cost  at  the  given  subgraph  size  to  the  correspond¬ 
ing  quantities  without  graduated  fidelity  (high  fidelity  throughout).  Averages  over  10  independent 
trials  are  presented. 


line  is  the  path  the  robot  traveled.  Note  that  it  explored  many  cul-de-sacs  due  to  the  limited  percep¬ 
tion,  and  the  planner  was  effective  at  guiding  the  robot  out  of  all  of  them,  while  leveraging  robot’s 
maneuverability.  The  replanning  due  to  obstacle  discovery  and  fidelity  modification  in  the  search 
space  was  occurring  continuously.  This  experiment  was  performed  on  a  conventional  laptop  com¬ 
puter  with  2GHz  CPU  and  2GB  of  RAM.  Notice  the  two  peaks  in  the  middle  of  the  runtime  plot, 
bottom  of  Figure  8.3.1:  they  correspond  to  two  replans  #39  and  #53,  when  significant  replanning 
was  required  due  to  cul-de-sacs. 

The  graduated  fidelity  motion  planner  was  integrated  with  research  prototype  rovers  at  the  Jet 
Propulsion  Laboratory  (JPL).  It  enabled  rovers  to  navigate  in  rough  rocky  terrain  set  up  in  the  JPL 
Mars  Yard.  Figure  8.3.2  shows  the  results  of  the  FIDO  rover  running  the  graduated  fidelity  state 
lattice  planner  on-board  to  navigate  autonomously  amid  dense  rocks.  In  this  experiment,  the  robot 
featured  a  single  1.6GHz  CPU  and  512MB  of  RAM,  shared  among  all  processes  of  the  rover.  The 
actual  memory  usage  of  the  planner  was  less  than  100MB.  The  rover  used  a  high  fidelity  region  of 
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Figure  8.2.2:  Simulated  robot  traversal:  limited  perception.  The  results  of  the  simulated  robot 
traversal  using  graduated  fidelity  as  in  Figure  8.2.1,  but  with  the  a  limited  pereeption  horizon. 
Averages  over  10  independent  trials  are  presented. 

the  same  size  as  above,  21x21  map  cells,  and  a  perception  region  (via  stereo  cameras)  41x41  map 
cells,  both  centered  around  the  rover.  The  top  part  of  Figure  8.3.2  shows  the  approximate  path  the 
rover  traveled,  and  the  bottom  part  shows  the  plot  of  the  runtimes  of  the  corresponding  replans, 
averaging  at  approximately  lOHz. 

8.4  Kinodynamic  Planning 

We  present  experimental  validation  results  of  kinodynamic  planning  using  two  examples:  a  double 
integrator  system  inspired  by  [29]  and  a  car-like  system  with  complex  dynamics.  We  also  describe 
a  multi-query  BVP  approach  (Section  7.1)  that  is  helpful  for  implementing  the  second  example, 
presented  in  Section  8.4.2.  Both  examples  were  developed  by  applying  the  design  principles  in 
Chapters  5-7.  The  primitives,  developed  with  Algorithm  7.3,  are  then  used  to  perform  incremental 
search  by  utilizing  the  unmodified  D*  Lite  algorithm  [71]  (Figure  8.4.3). 
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Figure  8.3.1:  Long-distance  simulated  forest  traversal.  A  simulated  experiment  of  traversing 
about  500  meters  among  previously  unknown  obstaeles.  Top:  the  first  40  meters  of  the  path  are 
shown  for  elarity.  Note  that  all  motions  generated  by  the  planner  were  globally  feasible,  and 
baeking  up  maneuvers  were  generated  automatieally  when  neeessary.  Bottom:  plot  of  planner 
runtime  (vertical  axis  is  runtime  in  seconds,  and  the  horizontal  axis  is  replan  cycle  number). 
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Figure  8.3.2:  JPL  Mars  Yard  experiment.  FIDO  rover  navigated  autonomously  among  pre¬ 
viously  unknown  obstacles,  while  running  the  graduated  fidelity  lattice  planner  on-board.  Top: 
approximate  path  the  rover  followed.  Bottom:  a  plot  of  planner  runtime  (vertical  axis  is  runtime  in 
seconds,  and  the  horizontal  axis  is  replan  cycle  number). 
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8.4.1  Double  Integrator 

The  double  integrator  is  a  simple  dynamies  system,  where  the  aeeeleration  is  eontrolled  direetly. 
Formally,  it  is  governed  by  a  seeond-order  differential  equation  x  =  u,  where  a:,  m  G  M.  We  look 
at  a  one-dimensional  example  here,  although  more  dimensions,  similarly  defined,  ean  be  added 
easily  due  to  their  independenee.  The  state  transition  equation  x  =  /(a:,  u)  can  be  written  as 
(ii,i2)  =  {x2:U).  Thus,  the  terminal  states  of  a  trajeetory,  XMt  and  Xgoai  are  speeified  by  the  eor- 
responding  positions  and  veloeities.  A  diserete-time  model  with  eontrols  given  byM  =  {  — 1,0, 1} 
has  reaehability  tree  trapped  on  a  lattiee  (Figure  8.4.1a). 

To  illustrate  the  applieation  of  Algorithm  7.3  to  this  system,  eonsider  an  example  in  Figure 
8.4.1b.  The  system  starts  at  zero  position  and  zero  veloeity  and  attempts  to  reaeh  a  speeified 
position  value  with  zero  terminal  veloeity.  The  trajeetory  in  this  example  is  the  sequenee  of  blaek 
arrows  in  part  b)  of  the  figure.  Part  e)  shows  a  few  of  the  elements  of  the  set  Ei  for  this  example; 
the  whole  set  is  prohibitively  large  to  illustrate.  Given  this  input,  the  algorithm  does  Dijkstra’s 
(line  2),  whieh  generates  the  sets  of  admissible  predeeessors  of  the  vertiees.  This  set  for  the  goal 
state,  Predc,  (xgoal)  is  shown  in  gray  in  Figure  8.4.1b.  Sinee  the  edges  between  all  the  states  in  this 
set  eome  from  the  original  seleetion  of  edges  allowed  in  this  problem  setup,  we  observe  that  the 
solution  Ea  eontains  the  same  set  of  edges  we  started  with  (Figure  8.4.  la).  In  the  ease  of  this  simple 
problem,  the  algorithm  is  able  to  find  the  optimal  solution  (that  is,  resulting  Ct  ratio  is  1.0),  however 
it  is  usually  not  the  ease  for  more  realistie  problems,  as  suggested  by  the  following  seetions.  The 
solution  path  (blaek  line  in  part  b)  of  the  figure)  ean  be  eonstrueted  with  the  replanning  seareh 
algorithms  we  eonsider  here. 

8.4.2  Car-like  Robot  WITH  Dynamics 

The  system  in  this  example  is  a  wheeled  robot  with  three  driven  wheels,  one  of  whieh  steers,  as 
shown  in  Figure  8.4.3b.  The  system  is  simulated  using  the  Open  Dynamies  Engine™software; 
the  system  model  is  not  available  in  elosed  form.  The  vehiele  has  signifieant  mass,  is  eapable 
of  aehieving  high  speeds  and  is  plaeed  on  a  very  slippery  flat  surfaee  to  highlight  the  effeets  of 
dynamies,  sueh  as  signifieant  drift,  sliding  sideways,  ete. 

A  eomparative  study  of  lattiee  and  high-diversity  primitives  [20;  32],  based  on  A*  seareh, 
showed  that  the  differenee  in  performanee  in  terms  of  runtime,  solution  quality  and  eompleteness 
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Figure  8.4.1:  Double  integrator  system  example. 


of  both  types  of  primitives  is  less  than  statistically  significant. 

The  benefit  of  the  principled  approach  here  is  that  the  length  of  primitives  is  selected  automat¬ 
ically,  while  it  is  fixed  for  path  diversity  primitives. 

Next  we  describe  experimental  validation  of  incremental  search  in  this  context,  although  other 
planning  approaches  may  benefit  from  such  primitives.  Figure  8.4.3  illustrates  an  example  where 
a  new  obstacle  invalidated  a  segment  of  the  previously  computed  trajectory.  D*  modified  the  plan 
efficiently  by  limiting  vertex  expansions  to  a  small  neighborhood.  The  initial  plan  was  computed 
in  1.42  seconds  (on  commodity  hardware),  and  was  repaired  in  0.35  seconds  -  a  nearly  4-fold 
speedup  with  respect  to  re-planning  from  scratch. 
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8.5  On-Demand  Expansion 

This  section  evaluates  on-demand  expansion  technique  in  the  context  of  state  lattices.  Figure  8.5.2 
demonstrates  path  alternatives  that  have  the  same  initial  and  final  state  as  the  canonical  path  (notice 
that  all  path  end-points  coincide).  In  many  applications  relevant  to  rover  navigation,  the  vehicle’s 
minimum  turning  radius  is  large  compared  to  the  desired  position  discretization  of  the  state  lattice. 
The  former  is  defined  by  the  maneuverability  of  the  vehicle’s  mobility  system,  while  the  latter  is 
influenced  by  a  number  of  factors,  most  notably  the  resolution  of  the  available  perception  system. 
While  the  maneuverability  of  planetary  rovers  is  fairly  high,  the  computation  constraints  inherently 
present  in  this  domain  disallow  very  fine  perception  resolution.  In  this  setting,  it  is  not  hard  to 
visualize  that  the  set  difference  between  the  canonical  path  and  its  alternatives  (number  of  black 
squares  in  Figure  8.5.2  will  be  either  small  or  zero).  For  example,  observe  that  the  lengths  of  the 
majority  of  motions  in  top  and  bottom  of  Figure  8.5.2  are  of  the  same  order  of  magnitude  as  the 
minimum  turning  radius,  2.5  cell  widths.  This  observation  explains  one  of  the  ’’lessons-leamed” 
of  this  effort:  retaining  the  identical  endpoints  of  canonical  and  alternative  paths  does  not  allow 
on-demand  expansion  to  yield  as  significant  impact  on  performance  as  allowing  the  path  endpoints 
to  vary.  The  only  criterion  for  including  paths  in  a  set  of  alternatives  was  to  retain  the  same  values 
of  final  state  other  than  the  position  dimensions.  In  particular,  we  constrained  this  set  to  have  paths 
with  the  same  final  heading  as  the  canonical  path. 

Figures  8.5.1,  8.5.3  and  8.5.4  demonstrate  experimental  results  of  the  algorithm  described  in 
the  previous  section.  Following  the  procedures  proposed  in  Section  5.2.1,  100  different  worlds  (of 
size  100x100  cells)  with  varying  single-point  obstacle  density  (5%  to  35%  density)  were  generated. 
Three  different  planners  were  used  to  generate  motions  to  every  cell  in  each  of  the  worlds  (resulting 
in  approximately  one  million  motion  solutions  per  density  level): 

1 .  A  planner  featuring  standard,  car-like  set  of  motion  primitives  with  the  graph  outdegree  that 
is  sufficiently  small  to  enable  the  planner  to  run  on-board  a  planetary  rover; 

2.  A  planner  with  an  artificially  high  outdegree  (comprising  all  motions  with  end-points  in  a 
large  region  around  the  rover,  13  by  13  cells); 

3.  A  planner  enabled  with  on-demand  expansion,  where  canonical  motions  were  taken  from 
the  primitives  of  Planner  1,  and  the  sets  of  path  alternatives  were  derived  from  Planner  2. 
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Figure  8.5.1  presents  plots  of  relative  runtime  and  cost  of  Planners  2  and  3,  presented  as  ratios 
to  the  respective  quantities  of  Planner  1 .  The  significant  result  is  that  the  coverage  and  cost  ratios 
of  Planner  3  are  very  similar  to  Planner  2,  while  its  computation  runtime  is  over  two  times  lower. 

Figure  8.5.4  makes  a  similar  presentation  of  comparing  coverage,  number  of  attainable  states, 
used  as  a  measure  of  completeness.  Figure  8.5.3  is  a  visual  representation  of  the  improved  coverage 
by  using  on-demand  expansion.  Blue  cells  are  obstacles,  and  shades  of  red  represent  the  number 
of  different  headings  that  are  attainable  at  each  cell  (dark  means  greater  number  of  headings). 

This  research  demonstrated  that  it  is  possible  to  generate  paths  in  a  recombinant  state  lattice 
using  a  control  set  whose  expressiveness  is  dependent  on  the  local  obstacle  density  of  the  envi¬ 
ronment.  This  provides  a  method  of  exploiting  computational  complexity  of  coarse  search  spaces 
while  gaining  some  the  expressiveness  of  denser  search  spaces.  In  the  domain  of  space/planetary 
robotics  (where  risk  minimization  and  resource  efficiency  are  important),  such  techniques  could 
provide  the  capability  to  plan  longer  distance  maneuvers  on  flight-analog  computers. 

8.6  Mobile  Manipulation 

The  presented  approach  was  evaluated  in  the  context  of  base  placement  with  differential  con¬ 
straints  in  mobile  manipulation.  This  is  recognized  as  a  hard  problem;  the  present  Section  attempts 
to  demonstrate  that  state  lattice  motion  primitives  offer  an  attractive  solution  in  this  context.  We 
present  results  of  three  types  of  experiments.  In  the  first  two,  the  mobile  manipulator  robot  is 
tasked  to  perform  “point- at”  operations:  the  target  pose  of  the  arm’s  end-effector  is  given.  The 
third  type  of  experiments  illustrates  the  “pick-and-place”  task,  where  object  coordinates  are  spec¬ 
ified  (in  SE{3)),  and  the  planner  must  compute  the  arm’s  end-effector  pose  that  would  allow  it 
to  grasp  the  object.  Figures  8.6.2  through  8.6.5  describe  the  experimental  setup.  Each  of  the 
experiment  types  was  repeated  20  times  (the  only  changed  variable  was  the  robot’s  initial  pose, 
selected  randomly).  The  base  planner  in  all  experiments  was  a  state  lattice  planner  running  A* 
[51],  configured  according  to  base  mobility  in  each  experiment.  It  was  enabled  with  a  heuristic 
look-up  table  (HLUT)  -  a  well-informed  heuristic  [109]  that  used  pre-computed  free-space  costs 
of  all  (discretized)  motions  in  the  area  that  spanned  the  workspace  of  each  experiment.The  first  two 
experiment  types  used  the  Resolution- Optimal  termination  condition  of  the  base  planner,  while  the 
third  type  used  Minimal-Runtime  termination  condition. 
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The  first  two  experiment  types  did  not  consider  arm  obstacles  (analytical  IK  was  used),  while 
the  third  experiment  type  used  Dijkstra’s  search  operating  in  five  dimensions  of  the  arm.  It  is 
expected  that  the  application  of  randomized  search  techniques  [13;  14;  87]  would  yield  faster  com¬ 
putation  time  for  arm  planning  in  this  setting.  The  experiments  were  executed  on  conventional 
computing  hardware:  a  laptop  computer  with  a  dual-core  2GHz  CPU  and  1GB  of  RAM.  The  run¬ 
time  statistics  of  all  trials  are  presented  in  Tables  8.2  and  8.3,  followed  by  a  summary  discussion. 

8.6.1  2D  Pointing 

This  simplified  experiment  is  intended  largely  as  an  illustration  of  the  concepts  presented  earlier. 
The  setup  is  inspired  by  the  JPL  Rocky8  rover,  pictured  on  the  left  of  Figure  8.6.2  [103].  A  mobile 
robot  base  has  six  wheels  and  is  capable  of  point  turns.  The  rover  features  a  5-DOF  PUMA-class 
arm,  although  it  is  simplified  in  this  section  as  a  2-DOF  planar  arm  for  consistency  with  earlier 
examples.  The  robot’s  “point-at”  task  is  to  place  the  arms’  end-effector  at  the  2D  coordinates  of 
the  target,  indicated  with  a  circle  and  a  cross  on  the  right  of  Figure  8.6.2.  The  is  also  shown, 
along  with  workspace  obstacles  and  the  motion  solution  of  the  planner  (red  line  with  dots).  The 
base  planner  was  a  state  lattice  planner  with  8-connected  grid  control  set,  configured  at  quarter- 
resolution  of  the  Gd  for  clarity.  The  red  dots  represent  the  vertices  in  the  graph  path,  and  red  lines 
are  the  edges. 

8.6.2  3D  Pointing 

A  more  realistic  set  of  experiments  in  this  sub-section  focuses  on  another  “point-at”  task,  where 
the  arm  end-effector  target  is  specified  in  SE  (2) .  This  type  of  experiments  builds  on  that  in  the 
previous  sub-section  and  extends  it  in  three  ways.  First,  the  base  is  subjected  to  car-like  kinematics 
constraints.  Second,  the  arm  is  enabled  with  the  third,  wrist,  joint;  this  arm  is  similar  to  Rocky 8 ’s, 
except  that  shoulder-yaw  and  wrist-yaw  joints  are  omitted  for  clarity  of  2D  illustration.  Third,  the 
manipulation  cost  is  defined  in  terms  of  application  of  a  force  along  the  specified  vector  in  2D.  This 
setup  models  the  task  of  drilling  or  riveting  in  construction  applications,  or  the  application  of  a  rock 
abrasion  tool  for  mineral  composition  experiments  in  planetary  exploration.  While  the  dynamics  of 
these  operations  is  not  considered  explicitly,  the  manipulation  cost  has  been  configured  to  represent 
a  heuristic  that  the  maximum  application  of  force  is  along  the  longitudinal  dimension  of  the  vehicle. 
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This  places  a  premium  on  base  poses  that  face  the  end-effector  target  (a  string  of  black  cells  leading 
to  the  target,  shown  as  a  circle  and  a  cross  in  the  right  half  of  Figure  8.6.3).  The  obstacle  in  front 
of  the  target  (set  of  white  cells)  is  considered  an  impediment  to  the  base  only;  the  arm  “goes  over” 
the  obstacle  to  perform  its  task  -  an  acceptable  solution  in  this  setup. 

8.6.3  Placing  I-Beams  in  Autonomous  Construction 

The  final  set  of  experiments  illustrate  “pick-and-place”  operation  (Figures  8.6.4  and  8.6.5).  The 
robot’s  task  is  to  install  I-beams  at  a  construction  site  that  is  too  large  and  constrained  to  allow  the 
use  of  a  fixed  manipulator  workcell.  The  arm  end-effector  pose  is  not  specified  explicitly;  instead 
SE{3)  pose  of  the  I-beam  is  given,  and  the  robot  must  compute  a  grasp.  In  our  experiments,  we 
used  a  heuristic  that  the  grasp  should  be  at  the  mid-point  of  the  beam,  oriented  along  its  height. 
Alternative  grasp  selection  methods  may  be  utilized  [13].  The  arm  is  a  PUMA-class,  5-DOF 
manipulator  with  a  1-DOF  gripper.  The  arm  is  installed  on  the  trailer  of  a  mobile  robot  with 
tractor-trailer  mobility.  The  system  must  operate  in  narrow  passages  among  obstacles. 

The  robot  entered  the  area  shown  via  a  side  entrance  while  carrying  the  beam  in  a  stowed  arm 
configuration:  the  shoulder  (longest)  link  was  oriented  along  the  longitudinal  axis  of  the  vehicle, 
and  the  elbow  link  was  folded  in.  The  base  planner  computed  the  footprint  of  the  vehicle  carrying 
the  beam  and  used  it  to  find  a  collision-free  path  (orange  dots  in  the  figure).  Once  the  base  arrived 
at  the  chosen  base  placement  pose,  the  elbow  link  unfolded,  while  the  shoulder  joint  kept  the 
shoulder  link  at  sufficiently  low  height  to  allow  the  arm  to  fit  between  the  long  yellow  beams  and 
place  the  I-beam  into  its  desired  pose. 

Table  8.2  presents  the  statistics  of  computational  runtimes  of  the  “point-at”  experiments.  The 
figures  are  mean  runtimes  over  all  runs  (in  seconds);  standard  deviation  figures  are  in  parentheses. 
Both  types  of  experiments  demonstrate  fast  computation  due  to  the  benefit  of  using  free-space,  ana¬ 
lytical  IK  for  arm  planning  and  a  lattice  planner  equipped  with  a  well-informed  HLUT  heuristic  for 
base  planning.  This  result  is  significant  because  it  demonstrates  computing  a  resolution-optimal, 
mobile  manipulator  motion  under  nonholonomic  constraints  for  a  realistic  “point-at”  task,  e.g. 
drilling  or  riveting  in  autonomous  construction,  among  arbitrary  base  obstacles  in  only  a  few  sec¬ 
onds.  The  result  is  likely  to  enable  fielded  solutions  of  this  type. 

Table  8.3  presents  the  statistics  of  “pick-and-place”  experiments.  We  separated  the  runtimes  for 
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Table  8.2:  Experimental  Planning  Runtime:  point-at  (seconds) 


2D  pointing 

3D  pointing 

0.37  (0.22) 

1.76  (0.37) 

Table  8.3:  Experimental  Planning  Runtime:  “pick-and-place”  (seconds) 


Arm  only 

Base  -1-  Cost  sum  min. 

Serial 

Parallel 

18.31  (5.46) 
12.76  (4.28) 

2.41  (0.47) 

2.87  (0.39) 

the  Dijkstra’s  arm  planner  (first  column)  and  the  rest  of  the  system.  The  total  runtime  of  the  whole 
system  is  the  sum  of  the  two  numbers.  The  separation  illustrates  that  nearly  90%  of  the  runtime 
was  due  to  the  arm  planner.  This  observation  is  significant  because  it  identifies  arm  planning  as  the 
unique  limiting  component.  Any  progress  in  improving  the  runtime  of  this  component  will  drive 
down  the  runtime  of  the  whole  mobile  manipulation  planning  system. 

The  first  row  of  the  table  presents  the  runtimes  when  the  mobile  manipulation  was  executed  in 
a  serial  fashion  on  the  single  core  of  the  CPU.  The  second  row  presents  analogous  figures  when 
the  computation  was  parallelized  between  the  two  cores  of  the  CPU.  Since  the  bulk  of  the  com¬ 
putation  was  arm  planning,  one  of  the  cores  executed  one  half  of  the  arm  planning  computations, 
while  the  other  core  executed  the  other  half,  as  well  as  the  rest  of  the  planning  system.  The  av¬ 
erage  runtime  reduction  is  not  two-fold  due  to  the  overhead  of  distributing  the  computation,  yet 
it  does  demonstrate  a  substantial  (over  40%)  reduction  in  runtime.  To  our  knowledge,  this  is  the 
first  result  of  this  type  in  the  given  domain.  Employing  a  greater  number  of  parallel  processes  is 
likely  to  improve  the  efficiency  further,  thereby  potentially  enabling  increased  adaptation  of  mobile 
manipulator  systems  in  a  variety  of  applications. 

8.6.4  Discussion 

In  this  section,  we  explored  motion  planning  for  mobile  manipulator  robots.  We  considered  a  class 
of  mobile  manipulation  problems  that  admit  decoupled  mobile  manipulation.  We  proposed  task- 
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space  decomposition  that  leverages  the  decoupled  nature  of  this  class  of  problems  and  yields  solu¬ 
tions  with  greater  efficiency  than  is  attainable  by  considering  the  original  problem.  Furthermore, 
the  components  of  the  decomposition  -  base  and  arm  planners  -  are  explicitly  coupled  via  user- 
defined  manipulation  cost,  which  allows  attaining  mobile  manipulator  motions  that  are  resolution- 
optimal  with  respect  to  that  cost.  In  addition,  the  approach  admits  the  flexibility  of  employing  a 
choice  of  component  planners,  including  those  that  satisfy  nonholonomic  constraints.  Application 
of  a  certain  type  of  base  planners  allows  parallelizing  mobile  manipulation  planning.  Experimental 
results  in  simulation  demonstrate  that  the  proposed  task-space  decomposition  is  competitive  with 
respect  to  the  state  of  the  art. 

In  addition,  it  would  also  be  interesting  to  validate  the  approach  on  physical  robots.  We  would 
also  like  to  explore  other  methods  of  achieving  dimensionality  reduction  in  this  setting,  as  well  as 
finding  ways  of  performing  a  greater  amount  of  off-line  pre-computation  in  order  to  gain  further 
efficiency  in  mobile  manipulation  planning. 
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Figure  8.4.2:  Control  set  regression.  Several  subsets  of  primitives,  generated  for  the  given  system, 
are  shown;  abundance  of  high  curvature  is  due  to  extreme  dynamics.  Top  and  bottom  rows  include 
primitives  at  low  and  high  velocity,  resp.  The  columns  show  the  primitives  with  final  headings  0°, 
90°  and  180°  (left  to  right).  Gray  motions  have  been  pruned,  as  concatenations  of  black  motions 
can  replicate  them  within  specified  cost  increase  threshold. 


Figure  8.4.3:  Kinodynamic  incremental  planning.  Robot  is  avoiding  a  number  of  obstacles 
(black  cells),  while  traveling  at  high  speed  on  slippery  surface.  A  new  obstacle  (dark  gray  cells)  is 
discovered  and  invalidates  a  segment  of  the  previous  trajectory  (dotted  line).  D*  repairs  the  path 
by  expanding  states  (light  gray)  only  in  the  affected  region. 
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Figure  8.5.1:  On-demand  expansion  performance.  Simulation  experiments  evaluating  the  per¬ 
formance  of  canonical,  expressive,  and  on-demand  expansion  control  sets.  Parts  a)  and  b)  compare 
the  relative  runtime  and  solution  cost,  respectively,  of  expressive  and  on-demand  expansion-based 
motion  sets  with  respect  to  canonical  motion  sets  (with  varying  obstacle  density).  Although  the 
expressive  motion  set  unsurprisingly  produces  the  safest  paths,  almost  the  same  quality  of  mo¬ 
tions  is  achieved  by  the  on-demand  expansion-based  motion  set  while  consuming  significantly  less 
computational  resources. 
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(d)  0,-  =  0°  (e)  0,  =  26.6°  (f)  0;  =  45° 


Figure  8.5.2:  Canonical  and  expressive  control  sets.  Top  row:  a  set  of  eanonieal  motion  prim¬ 
itives  that  represents  the  minimal  expressiveness,  yet  yields  fast  planning  due  to  its  small  size. 
Bottom  row:  the  other  end  of  the  speetrum:  high  expressiveness,  yet  longer  planning  runtime. 


CHAPTER  8.  EXPERIMENTAL  RESULTS 


131 


Figure  8.5.3:  Coverage  pseudo-color.  The  figure  illustrates  the  eapaeity  of  the  robot  to  move  to 
loeations  that  are  hard  to  aehieve  due  to  dense  obstaeles.  The  pseudo-eolor  represents  the  num¬ 
ber  of  different  headings  that  are  attainable  for  eaeh  eell  in  the  environment  using  the  eanonieal 
template  (left)  and  on-demand  expansion  (right).  Shades  of  red  denote  the  number  of  attainable 
headings  (deep  red  represents  all  desired  headings),  while  blue  eells  are  obstaeles. 
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Figure  8.5.4:  Relative  coverage.  A  plot  of  the  number  of  attainable  states  using  Planners  2  and  3, 
represented  as  the  ratios  to  the  eorresponding  quantities  of  Planner  1 .  The  plot  demonstrates  that 
on-demand  expansion  (solid  line)  features  a  similar  improvement  over  the  canonieal  motion  set 
as  the  expressive  motion  set,  while  allowing  significantly  lower  computation  cost,  as  suggested  in 
Figure  2a). 


Figure  8.6.1:  Discretized  goal  region.  Figure  a)  shows  a  two-link  planar  arm  installed  on  a  mobile 
base.  Figure  b)  illustrates  a  subset  of  the  goal  region  G  induced  by  p(«),  where  grayscale  intensity 
corresponds  to  its  value  (white  color  indicates  p(«)  =  oo).  An  odd-shaped  obstacle  is  visible  near 
the  manipulation  target  T .  Figure  c)  shows  the  discretized  version  of  the  goal  region,  G^.  The 
obstacle  causes  a  set  of  cells  to  be  white  -  infeasible  poses  for  the  base  joint  of  the  arm. 
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Figure  8.6.2:  2D point-at  task.  Left  subfigure  shows  the  RockyS  rover  that  inspired  this  and  the 
following  subsection  (courtesy  of  JPL  [103]).  The  figure  on  the  right  illustrates  mobile  manipula¬ 
tion  in  the  scenario  where  the  robot  moves  from  top  right  of  the  figure  (arm  is  stowed  initially)  to 
the  pose  that  maximizes  manipulability.  Note  that  the  base  joint  of  the  arm  lands  in  the  black  cell, 
indicating  minimal  manipulation  cost).  The  red  line  with  dots  is  the  motion  of  the  base,  computed 
by  the  planner.  The  dots  are  the  vertices  of  the  graph  (elaborated  by  the  planner),  and  the  lines  are 
the  edges.  The  motion  represents  the  resolution-optimal  trade-off  between  path  cost  (length)  and 
manipulation  cost  (manipulability)  automatically  made  by  the  planner. 


Figure  8.6.3:  3D  point-at  task.  Left  subfigure  is  intended  to  motivate  this  class  of  manipulation 
tasks;  it  shows  a  close-up  view  of  the  Rocky8  rover’s  arm  performing  a  rock  imaging  study,  com¬ 
mon  in  planetary  exploration  (courtesy  of  JPL  [103]).  The  figure  on  the  right  illustrates  a  more 
realistic  set  of  experiments  that  extend  Figure  8.6.2.  The  initial  pose  of  the  rover  is  shown  in 
top  right  (with  arm  stowed).  The  inset  shows  the  control  set  utilized  by  the  state  lattice  nonholo- 
nomic  planner  [109];  an  identical  set  of  controls  for  reverse  motion  is  omitted  for  conciseness. 
The  red  line  with  dots  represents  the  solution  of  the  planner,  where  the  dots  are  the  vertices  of 
the  graph,  elaborated  by  the  planner,  and  the  lines  are  the  edges.  Similar  to  Figure  8.6.2,  the  mo¬ 
tion  represents  the  trade-off  between  path  cost  (length)  and  manipulation  cost  (manipulability), 
automatically  made  by  the  planner. 
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Figure  8.6.4:  Placing  I-beams  in  autonomous  construction.  The  proposed  approach  is  well- 
suited  for  “pick-and-place”  and  other  common  manipulation  tasks.  In  this  example,  the  robot’s 
task  is  to  install  I-beams  at  a  construction  site  that  is  too  large  and  constrained  to  allow  the  use 
of  a  fixed  manipulator  workcell.  The  motion,  computed  by  the  planner,  represents  a  resolution- 
optimal  trade-off  between  path  length,  traversed  by  the  tractor-trailer  platform,  and  the  subsequent 
manipulator  operation. 


Figure  8.6.5:  A  pick-and-place  experiment.  The  robot’s  task  is  to  install  red  I-beams  between  long 
yellow  beams  at  a  construction  site.  A  5-DOF,  PUMA-class  arm  with  a  1-DOF  gripper  is  installed 
on  the  trailer  of  a  mobile  robot  with  tractor-trailer  mobility.  The  system  must  operate  in  narrow 
passages  among  obstacles.  The  figure  shows  the  base  placement  of  the  robot  computed  by  the 
planner,  given  the  desired  pose  for  placing  the  I-beam  object.  Orange  dots  show  the  base  planner 
solution,  the  trajectory  of  the  tractor  robot.  Left  and  right  subfigures  show  different  viewpoints  of 
the  scene. 


CHAPTER 


CONCLUSIONS 


This  work  has  been  motivated  by  a  fairly  acute  need  to  endow  our  field  robots  with  sufficient  un¬ 
derstanding  of  their  own  mobility  to  allow  them  to  efficiently  plan  correct  and  intricate  maneuvers 
in  response  to  their  challenging  surroundings. 

This  thesis  proposed  an  approach  of  encapsulating  the  complexity  of  differential  constraint 
satisfaction  via  a  novel  type  of  structured,  pre-computed  controls  that  serve  as  motion  primitives. 
The  contribution  of  this  work  is  in  developing  a  general  approach  to  constructing  such  motion 
primitives,  given  a  model  of  robot  mobility. 

Moreover,  the  proposed  type  of  motion  primitives  allows  an  unprecedented  amount  of  pre- 
computation  in  the  area  of  differentially  constrained  motion  planning,  which  yields  a  dramatic 
increase  in  planning  efficiency  even  for  systems  with  complex  kinematics  and  dynamics.  Lastly, 
the  proposed  motion  primitives  are  fully  compatible  with  a  wide  range  of  planning  algorithms  and 
allow  such  useful  techniques  as  incremental  planning  and  bi-directional  search  to  be  used  in  the 
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context  of  planning  with  differential  constraints. 

In  conclusion,  the  primary  contribution  of  this  work  is  not  a  motion  planner,  but  a  principled, 
automated  mechanism  to  design  an  efficient,  precision,  differentially  constrained  search  space 
upon  which  any  planner  may  operate.  A  compelling  case  was  presented  regarding  the  benefits 
of  the  approach  with  respect  to  the  prevailing  alternatives.  The  proposed  method  has  been  demon¬ 
strated  and  validated  on  a  number  of  relevant  systems,  both  in  simulation  and  in  real  experiments. 
This  work  promises  to  enable  capable  and  reliable  motion  planners  with  differential  constraints,  as 
encountered  in  many  realistic  robot  systems  with  practical  utility,  operating  efficiently  in  cluttered, 
partially  known  environments. 
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